perm filename ARM.PAL[OLD,HE] blob
sn#492422 filedate 1980-01-03 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00074 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00009 00002 PDP11/45 ARM SERVO PROGRAM: REVISIONS
C00013 00003 CONDITIONAL ASSEMBLY FLAGS
C00018 00004 DEFINITIONS
C00022 00005 MACROS FOR ESTABLISHING RELATIVE POINTERS FOR DEVICE DATA BLOCK
C00024 00006 MACROS TO ESTABLISH POINTER TABLES INTO ARM SERVO DATA BLOCKS
C00025 00007 POINTER TABLES FOR SERVO DATA
C00028 00008 SERVO FUNCTION ERROR CODES & BIT MASKS FOR SERVO, STATUS AND MODE WORDS
C00032 00009 ORGANIZATION OF SERVO DATA BLOCK
C00039 00010 DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
C00044 00011 SERVO - START OF LEVEL 7 SERVO CODE
C00054 00012 ROUTINES FOR STOPPING JOINT MOTION
C00056 00013 ANGLES - CONVERTS A/D READINGS TO JOINT ANGLE AND VELOCITY
C00062 00014 FEEDBK - SERVO FEED BACK LOOP TO DRIVE THE JOINT MOTOR
C00069 00015 SCHED - SCHEDULES SERVO TO RUN AGAIN
C00071 00016 EVAL - FETCHES POLYNOMIAL COEF. AND EVALUATES FOR NEXT SERVICING TIME
C00084 00017 WIPER - SWITCHES A/D CHANNELS IF A/D OUT OF RANGE AND SERVO HAS MULTIPLE WIPERS
C00087 00018 MOTDRV,MOTSTP,BRKREL - MOTOR DRIVE ROUTINES
C00090 00019 INTARM - INITIALIZES SERVO CODE FOR FUTURE ARM MOTIONS
C00098 00020 DRIVE - INITIATES MOTIONS FOR ONE JOINT, SINGLE SEGMENT
C00103 00021 CENTER - CLOSES THE HAND FINGERS AND CENTERS THE ARM OVER ANY GRASPED OBJECT
C00113 00022 CNTRRT & CNTRLF - ON MONITOR FOR CENTER FUNCTION
C00117 00023 DATA FOR CENTER OPERATIONS
C00120 00024 WHERE - UPDATES THE CURRENT JT ANGLE AND DYNAMIC COEF FOR SERVOS
C00124 00025 MOVE - MOVES THE ARM JOINTS ALONG A PRECOMPUTED TRAJECTORY
C00132 00026 ["MOVE"]
C00140 00027 ["MOVE"]
C00146 00028 ["MOVE"]
C00151 00029 MOVE - DIAGNOSTIC MOVE INSTRUCTION WITHOUT TRAJECTORY MODIFICATION
C00158 00030 MAK5TH - CONSTRUCTS THE 5TH ORDER POLY. USED TO MODIFY RUNTIME TRAJECTORIES
C00164 00031 ATTSRV - ROUTINE FOR ATTACHING SERVOS
C00171 00032 RELSRV - ROUTINE FOR DETACHING SERVOS
C00174 00033 SETSET - RESETS SERVO BLOCK VARIABLES AFTER A MOVE IS COMPLETED
C00176 00034 SETTH - READS A/D AND INITIALIZES JOINT ANGLES FOR A GIVEN DEVICE BLOCK
C00185 00035 SETREF - READ A/D, SET REFERENCE VOLTAGE AND ZERO TACH VALUE READINGS
C00192 00036 STHLV7 & SRFLV7 - LEVEL 7 CODE FOR "SETTH" AND "SETREF"
C00197 00037 RUNSRV - SCHEDULES THE SERVOS OF A DEVICE TO RUN AND WAITS FOR THEIR COMPLETION
C00205 00038 DACSER - BLUE ARM DAC REFRESH LEVEL 4 ROUTINE AND ARM ERROR TRAP ROUTINE
C00208 00039 TOUCH - SCHEDULES TOUCH EVENTS
C00214 00040 CLRTCH - TAKES A SCHEDULED EVENT OFF OF THE TOUCH SENSOR EVENT LISTS
C00217 00041 SMPTCH,STPTCH - INITIATES AND TERMINATES UPDATING OF THE TOUCH SENSORS READINGS
C00219 00042 TCHLV7 - LEVEL 7 CODE FOR TOUCH SENSORS
C00225 00043 FORCE - SUBRS. USED FOR FORCE SENSING AND COMPLIANCE
C00228 00044 SETC - INITIALIZES FORCE SENSING/COMPLIANCE SYSTEM
C00232 00045 FRCSIG - INITIALIZES JOB STARTING BASED UPON A FORCE READING
C00236 00046 FRCOFF - TAKES A QUEUED JOB OFF OF THE FORCE SIGNAL LIST
C00238 00047 COMPLY - SETS UP FORCE COMPLIANCE OF A GIVEN MAGNITUDE AND DIRECTION
C00241 00048 CMPOFF - TURNS OFF FORCE COMPLIANCE IN A SPECIFIED DIRECTION
C00243 00049 FRCLV7 - LEVEL 7 CODE FOR FORCE SENSING AND COMPLIANCE
C00258 00050 FBACK - LEVEL 5 FORCE SENSING BACKGROUND JOB
C00264 00051 ["FBACK"]
C00269 00052 ["FBACK"]
C00275 00053 DEFINITIONS FOR PROGRAMS USING WRIST SENSOR
C00276 00054 SETBAS - PROCEDURE TO SET THE BASE READINGS FOR THE FORCE WRIST
C00283 00055 WRIST - PROCEDURE TO READ AND RESOLVE THE FORCE WRIST READINGS
C00290 00056 GATHER - PROCEDURE TO COLLECT FORCE DATA
C00297 00057 FORCE SENSING ROUTINES DATA AREA
C00307 00058 VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
C00309 00059 SRV01: NONEX YELLOW ARM JOINT #1 DATA
C00312 00060 SRV02: NONEX YELLOW ARM JOINT #2 DATA
C00315 00061 SRV03: NONEX YELLOW ARM JOINT #3 DATA
C00318 00062 SRV04: NONEX YELLOW ARM JOINT #4 DATA
C00321 00063 SRV05: NONEX YELLOW ARM JOINT #5 DATA
C00324 00064 SRV06: NONEX YELLOW ARM JOINT #6 DATA
C00327 00065 SRV07: NONEX YELLOW ARM HAND SERVO DATA BLOCK
C00330 00066 SRV08: 0 BLUE ARM JOINT #1 DATA
C00333 00067 SRV09: 0 BLUE ARM JOINT #2 DATA
C00336 00068 SRV10: 0 BLUE ARM JOINT #3 DATA
C00339 00069 SRV11: 0 BLUE ARM JOINT #4 DATA
C00342 00070 SRV12: 0 BLUE ARM JOINT #5 DATA
C00345 00071 SRV13: 0 BLUE ARM JOINT #6 DATA
C00348 00072 SRV14: 0 BLUE ARM HAND SERVO DATA BLOCK
C00351 00073 SRV15: 0 VISE SERVO DATA BLOCK
C00354 00074 DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
C00356 ENDMK
C⊗;
; PDP11/45 ARM SERVO PROGRAM: REVISIONS
.TITLE ARM
.EVEN
;Feb 9, 1979:
; GATHER and force gathering system made operational
;Jan. 19, 1979:
; WRIST and SETBASE made operational
;Aug. 31, 1978:
; DTERMS in BEJCZY.PAL modified to explicitly zero gravity loading for
;joint 1 and 6.
;May 11, 1978:
; Added call to where at beginning of center to permit manual positioning
;of arm prior to center. Fixed velocity error nulling bug in SERVO.
;December 6, 1977:
; Panic Button interupt service routine now returns via a new kernel
;system call, KRTI, permitting interupt returns from outside supervisor mode.
;November 6, 1977
; Added filtering to force signalling
;March 14, 1977:
; Changed Blue arm interface dac refresh routine to level 4 from 7
;and removed restriction on running only one device at a time.
;March 8, 1977:
; Re-calibrated blue arm after all dead tachs repaired and arm
;generally re-built.
;October 27, 1976:
; Changed some symbols to locals, put conditional compilation
;switches on linear calibration data and drive. Added WOBBLE.
;August 2,1976:
; Power supply drifting by as much as 4 counts, upped reference
;error tolerance to ignor for now.
;May 12, 1976:
; Adding procedures WRIST and SETBAS for reading and resolving
;the force wrist strain gage values.
;April 23, 1976:
; Bug in FEEDBACK subroutine corrected. Used to store the error
;torque ( ERRTQE ) in funny units, not oz-in and oz's. Also changed
;EVAL so that the inertia torque left in AC3 was in the proper units.
;March 25,1976:
; KV,KE,KI changed for BLUE arm, joint #2. ERRTOL changed from
;.05 to .15 for joint #2, .01 to .02 for joint #3, due mainly to a hardware
;bug in the ADC. Only odd ADC readings seem to be stable for these joints.
;March 20, 1976:
; Arm code uses new Kernel, does its own ADC reading. Order of
;execution altered so that polynomial evaluation is done before the
;pots and tachs are read and before the error feedback is computed.
;CONDITIONAL ASSEMBLY FLAGS
;MACRO FOR DEFINING UNDEFINED CONDITIONAL ASSEMBLY SWITCHES
.MACRO DSWT SWTCH,DEFULT
.IFNDF SWTCH
.PRINT /"SWTCH" ASSEMBLY SWITCH NOT DEFINED, USING DEFAULT VALUE
/
SWTCH==DEFULT
.ENDC
.ENDM
DSWT SNGSTP,0 ;SINGLE STEP MODE, DOES NOT CALL SCHEDULER OR SET DAC/ADC
DSWT DIAGY,1 ;DIAGNOSTIC MODE, JOINT VARIABLES SAVED IN BUFFER DBUF
DSWT STDALN,1 ;STAND ALONE, DOES NOT REQUIRE AL GRAPH ROUTINES
DSWT TACCAL,0 ;MODE TO CALIBRATE THE TACHOMETERS
DSWT NOYELW,1 ;NO YELLOW ARM YET, DON'T CREATE SERVO DATA BLOCKS
DSWT TIMER,0 ;SAVES EXECUTION TIME OF SERVO CODE IN ARRAY "TIMES"
DSWT ISLIN,1 ;JOINTS ASSUMED TO BE LINEAR, DON'T DO NON-LINEAR INTERP.
DSWT FRCDAT,0 ;SAVE FORCE SENSING SYSTEM DATA
DSWT SHORT,0 ;SET TO 1 ELIMINATE UNNECESSARY STUFF FOR SENSOR DIAGNOSTICS
;HEADER SECTION FOR ASSEMBLING "ARM" FOR OVERLAYING WITH AL ROUTINES
.IFZ STDALN
.OFFSET -160000 ;Put it in the high memory
.IF1
.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.ENDC
.OFFSET -340000 ;Put it in the high memory
.=ARMCOD ;START ADDRESS OF SERVO CODE
CODE$ == ARMCOD
DATA$ == ARMDAT
SPSWITCH == 0 ;Make sure we start off with everyone properly defined
DATA
.BLKW 96. ;RESERVE SPACE FOR WRIST CALIBRATION DATA
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARMSOL.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
DATA
PUTLOC ARMVER,VERSION ;TELL AL WHAT VERSION NUMBER THE BIN FILE IS
PUTLOC LINTARM,INTARM ;ESTABLISH POINTERS TO ROUTINES THAT AL CALLS
PUTLOC LCENTER,CENTER
PUTLOC LWHERE,WHERE
PUTLOC LMOVE,MOVE
PUTLOC LSQRTF,SQRTF
PUTLOC LEXP,EXP
PUTLOC LLOG,LOG
PUTLOC LSNCSD,SNCOS
PUTLOC LASIN,ASIN
PUTLOC LACOS,ACOS
PUTLOC LATAN2,ATAN2
PUTLOC LERRPTR,ERRPTR
PUTLOC LTHPTR,THPTR
PUTLOC LDVCPTR,DVCPTR
PUTLOC LUPDATE,UPDATE
PUTLOC LSETBAS,SETBAS
PUTLOC LWRIST,WRIST
PUTLOC LSETC,SETC
PUTLOC LFRCSIG,FRCSIG
PUTLOC LFRCOFF,FRCOFF
PUTLOC LCOMPLY,COMPLY
PUTLOC LCMPOFF,CMPOFF
PUTLOC LPOTPTR,POTPTR
PUTLOC LGATHER,GATHER
.ENDC
;DATA POINTER DEFS.
IIII == .
.==NOTB11
.WORD 0
PCDBUF::.WORD 0 ;pcode buffer address
INTBUF::.WORD 0 ;integer buffer starting address
FPBUF:: .WORD 0 ;floating point buffer starting address
INTPTR::.WORD 0 ;current position of integer pointer
FPPTR:: .WORD 0 ;current position of fp buffer
INTSIZ::.WORD 0 ;size of integer buffer
FPSIZ:: .WORD 0 ;size of fp buffer
.==IIII
;END OF HEADER SECTION FOR OVERLAYING WITH AL ROUTINES
;DUMMY MACRO DEFINITIONS FOR STAND ALONE VERSION
.IFNZ STDALN
.MACRO CODE
.ENDM
.MACRO DATA
.ENDM
.ENDC
;DEFINITIONS
AC =%0 ;GENERAL ACCUMULATOR
BC =%1 ; " "
CC =%2 ; " "
DC =%3 ; " "
EC =%4 ; " "
DAT =%5 ;CONTAINS ADDRESS OF FIRST WORD OF SERVO DATA
FC =%5
SP =%6 ;STACK POINTER
PC =%7 ;PROGRAM COUNTER
;NUMBER REPRESENTATION OF VARIABLES AND CONSTANTS:
;
;JOINT ANGLES: DEGREES
;ANGULAR VELOCITY: DEGREES/MILLISECOND
;ANGULAR ACCELERATION: DEGREES/MILLISECOND**2
;TIME: TICKS {APPROXIMATELY 1 MILLISECOND}
;TORQUE: OZ-INCHES OR OZ
;PROCESSOR PRIORITY LEVELS CURRENTLY BEING USED:
; LEVEL 7: SERVO JOBS, FORCE SENSING TRIGGER JOB, TOUCH SENSING
; TRIGGER JOB, SETTH ADC READER, SETREF ADC READER
; LEVEL 6: NOT USED
; LEVEL 5: CENTER ON-MONITOR, FORCE SENSING AND COMPLIANCE CALCULATOR
; LEVEL 4: BLUE ARM DAC RE-FRESH JOB, FORCE SENSING ERROR MESSAGE RTN
;YELLOW INTERFACE DEFS
DRATRP ==530 ;DR11 A VECTOR
DRBTRP ==534 ;DR11 B VECTOR
DR11S =167770 ;DR11 STATUS WORD
DR11O =167772 ;DR11 OUTPUT REGISTER
DR11I =167774 ;DR11 INPUT REGISTER
PANICB =200
ENABLE =100000 ;INTERFACE ENABLE (was 400)
;BLUE INTERFACE DEFS
DACCSR =174000 ;DAC STATUS AND CONTROL WORD ADDRESS
ADCSR =174004 ;ADC CONTROL AND STATUS WORD ADDRESS
ADCCHN =174005 ;ADC CHANNEL ADDRESS
ADCVAL =174006 ;ADC VALUE ADDRESS
DACDNE ==200 ;DAC DONE BIT
;RANDOM DEFINITIONS
DBUFL ==6000. ;LENGTH OF DIAGNOSTIC BUFFER
MAXSRV ==15. ;NUMBER OF EXISTING SERVOS **K CHANGED TO 15. FOR VISE
NULTME ==5000. ;NUMBER OF MILLISECONDS ALLOWED TO NULL FINAL POS. ERROR
STPTME ==1000. ; " " " " FOR ARM TO STOP AFTER AN
; ERROR HAS OCCURRED
CTRSTP ==1500. ;NUMBER OF MILLISECONDS ALLOWED FOR HAND TO CLOSE AFTER
; BOTH TOUCH SENSORS ACTIVATED IN CENTER FUNCTION
;REFERENCE POWER SUPPLY DATA
DATA
REFCN1 ==16 ;ADC CHANNELS FOR REFERENCE READING
REFCN2 ==17
REFER1: -1007. ;POWER SUPPLY READING AT TIME OF JOINT CALIBRATION
REFER2: 1021.
REFVT1: -1007. ;CURRENT POWER SUPPLY READINGS
REFVT2: 1021.
REFTOL ==6 ;ERROR TOLERANCE PERMITTED BEFORE "BAD REF." IS SIGNALLED
;MACROS FOR ESTABLISHING RELATIVE POINTERS FOR DEVICE DATA BLOCK
;INT ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 2
.MACRO INT SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+2
.ENDM
;BYTE ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 1
.MACRO BYTE SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+1
.ENDM
;REAL ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 4
.MACRO REAL SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+4
.ENDM
;LSKIP INCREMENTS THE RELATIVE POINTER "RELCNT" BY 2*N
.MACRO LSKIP N
.REPT N
RELCNT == RELCNT+2
.ENDR
.ENDM
;MACROS TO ESTABLISH POINTER TABLES INTO ARM SERVO DATA BLOCKS
.MACRO YELPTR LABEL
YJTPTR LABEL
SRV07+LABEL
.ENDM
.MACRO YJTPTR LABEL
SRV01+LABEL
SRV02+LABEL
SRV03+LABEL
SRV04+LABEL
SRV05+LABEL
SRV06+LABEL
.ENDM
.MACRO BLUPTR LABEL
BJTPTR LABEL
SRV14+LABEL
SRV15+LABEL
.ENDM
.MACRO BJTPTR LABEL
SRV08+LABEL
SRV09+LABEL
SRV10+LABEL
SRV11+LABEL
SRV12+LABEL
SRV13+LABEL
.ENDM
;POINTER TABLES FOR SERVO DATA
DATA
SERVOS: ;POINTERS TO SERVO LISTS, ORDER FOLLOWS BIT ASSIGNMENTS
YSRVOS: YELPTR 0 ;YELLOW ARM AND HAND JOINTS
BSRVOS: BLUPTR 0 ;BLUE ARM AND HAND JOINTS AND VISE
;POINTERS TO ARM JOINT ANGLES
THPTR:
YTH: YELPTR TH ;POINTERS TO CURRENT YELLOW ARM JOINT ANGLES
BTH: BLUPTR TH ; " " " BLUE " " "
THPPTR:
YTHP: YELPTR THP ; " " PREDICTED YELLOW " " "
BTHP: BLUPTR THP ; " " " BLUE " " "
;POINTERS TO RAW POT READINGS
POTPTR: YELPTR POT
BLUPTR POT
;TABLE OF POINTERS TO YELLOW ARM DYNAMIC COEFFICIENTS
YCI: YJTPTR CI ;CURRENT GRAVITY LOADING TERMS
YJTPTR CII ; " JOINT INERTIA TERMS
YNCI: YJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
YJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO BLUE ARM DYNAMIC COEFFICIENTS, CI & CII
BCI: BJTPTR CI ;CURRENT GRAVITY LOADING TERMS
BJTPTR CII ; " JOINT INERTIA TERMS
BNCI: BJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
BJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO ERROR TORQUE TERMS
ERRPTR: YELPTR ERRTQE ;POINTERS TO JOINT ERROR TORQUE TERMS
BLUPTR ERRTQE ; " " " " " "
;TABLE OF POINTERS TO SERVO DEVICE BLOCKS
DVCPTR: YELPTR INUSE ;POINTERS TO DEVICES TO WHICH SERVOS ARE ATTACHED
BLUPTR INUSE
;SERVO FUNCTION ERROR CODES & BIT MASKS FOR SERVO, STATUS AND MODE WORDS
;THE FOLLOWING ARE THE POSSIBLE ERROR CODES THAT CAN BE RETURNED IN R0 AFTER
;A SERVO FUNCTION HAS BEEN EXECUTED, EG. MOVE,CENTER.
CNTATT==1 ;COULD NOT ATTACH TO REQUESTED JOINT(S)
WRGNUM==2 ;INCORRECT NUMBER OF JOINTS REQUESTED TO BE DRIVEN
BADWIP==3 ;WIPERS COULD NOT BE READ WITHIN THEIR OPERATING RANGE
NOSOLU==4 ;ARM SOLUTION DOES NOT EXIST
UNKTCH==5 ;UNKNOWN TOUCH SENSOR REQUESTED
TOMTCH==6 ;NO MORE FREE SLOTS IN TOUCH SENSOR EVENT LIST
NOPOWR==7 ;ARM INTERFACE POWER SUPPLY TURNED OFF
BADREF==10 ;REFERENCE POWER SUPPLY OUT OF RANGE
BADTAC==11 ;ZERO VELOCITY TACHOMETER READING OUT OF RANGE
WRGARM==12 ;ATTEMPTED TO SWITCH ARMS WHILE FORCE SERVOING
TOMFRC==13 ;NO MORE FREE SLOTS IN FORCE SENSOR EVENT LIST
FWRGJT==14 ;NEED ALL 6 ARM JOINTS IN ORDER TO DO FORCE SENSING/COMPLIANCE
NOPOLY==15 ;CAN'T FORCE SERVO MOTION WITHOUT POLYNOMIAL
BOVERR==16 ;BACKGROUND JOB TOOK TOO LONG TO EXECUTE
;IF NO ERROR OCCURRED, A ZERO IS RETURNED. ANY OTHER NON-ZERO R0 VALUES
;CORRESPONDS TO THE STATUS WORD BIT ERROR FLAGS PRESENTED BELOW, EG. NONEX,
;ADERR.
;STATUS WORD BIT MASKS
NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;!!!!!!DONT CHANGE BIT ASSIGNMENTS OF "NXTFIN"&"FINAL"!!!!!!
RUN ==4 ;RUN SERVO, SET TO ZERO TO STOP SERVOING
MOVING ==10 ;DEVICE IS IN MOTION, I.E. THE VELOCITY IS NOT KNOWN TO BE ZERO
STROUT ==20 ;JOINT STARTED OUTSIDE OF PERMITTED OPERATING RANGE
FIRST ==40 ;FIRST PASS THRU SERVO
FSERVO ==100 ;FORCE SERVO THIS JOINT
NONEX ==400 ;JOINT IS DOWN, INOPERABLE
ADERR ==1000 ;CATASTROPHIC A/D ERROR HAS OCCURRED
PANCBT ==2000 ;PANIC BUTTON WAS PUSHED
EXJTFC ==4000 ;EXCESSIVE JOINT FORCE ERROR
TMEOUT ==10000 ;FUNCTION TOOK TOO LONG TO EXECUTE
PASLIM ==20000 ;JOINT STOPPED BEYOND STOP LIMIT
FNOSOL ==40000 ;NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE
;MODE WORD BIT MASKS
NNUL ==1 ;DO NOT NULL POSITION ERROR AT THE END OF A TRAJECTORY
WOBBLE ==2 ;WOBBLE OUTER JOINTS AT END OF TRAJECTORY
DEPTPT ==4 ;MOTION HAS A DEPARTURE POINT
;BIT MASKS TO TEST FOR BLUE OR YELLOW HAND IN POLYNOMIAL BLOCK HEADER
YHAND ==1000 ;YELLOW HAND
BHAND ==4 ;BLUE HAND
;BIT MASKS TO TEST FOR BLUE OR YELLOW ARM JOINTS IN POLYNOMIAL BLOCK HEADER
YARM ==176000;YELLOW ARM
BARM ==770 ;BLUE ARM
;BIT MASKS TO TEST FOR VISE AND SCREWDRIVER IN POLYNOMIAL BLOCK HEADER
BVISE ==2 ;VISE
BSCREW ==1 ;SCREWDRIVER
;ORGANIZATION OF SERVO DATA BLOCK
;RELATIVE ADDRESSES FROM START OF SERVO DATA BLOCK
RELCNT ==2 ;FIRST WORD CONTAINS STATUS BITS
INT MODBTS ;OPERATION MODE BITS, EG. WOBBLE, NNUL
INT INUSE ;0 IF SERVO NOT ATTACHED, ELSE POINTER TO DEVICE BLOCK.
BYTE SRVNUM ;LOGICAL NUMBER OF SERVO
BYTE MECHSM ;BIT INDICATING WHICH MECHANISM THIS SERVO IS A PART OF(SEE BELOW)
REAL TH ;CURRENT JOINT ANGLE READ FROM POTS IN DEGREES
REAL THP ;PREDICTED JOINT ANGLE IN DEGREES
REAL THN ;PREDICTED JOINT ANGLE FOR NEXT SERVOING PERIOD, IN DEGREES
REAL THD ;CURRENT ANGULAR VELOCITY OF JOINT IN DEGREES/TICK
REAL THDP ;PREDICTED ANGULAR VELOCITY IN DEGREES/TICK
REAL THDN ;PREDICTED ANGULAR VELOCITY FOR NEXT SERVOING PERIOD, IN DEGREES
REAL DELTH ;TOTAL DEVIATION OF JOINT ANGLE DUE TO DELTHD
REAL DELTHD ;VELOCITY FACTOR FOR MODIFYING TRAJECTORY WHILE IN MOTION
REAL POSERR ;POSITION ERROR, TH-THP
INT WOBPTR ;INDEX INTO WOBBLE SINUSOIDAL TABLE, -1 FOR JTS THAT DONT WOB.
INT WOBMAG ;PTR TO MAGNITUDE OF WOBBLE IN JTS 4,5,6
INT DATPT ;PTR TO START OF POLYNOMIAL DATA LIST
INT POLY ;PTR TO POLYNOMIAL A5 COEF. FOR THIS SERVO
INT FCI ;PTR TO FINAL JOINT GRAVITY LOADING FOR SEGMENT
INT SDTIME ;MINIMUM TIME BETWEEN SCHEDULING SERVO FOR RUNNING, IN TICKS
INT TTIME ;TOTAL TIME OF PRESENT SEGMENT, IN TICKS
REAL FTTIME ; " " " " " " "
INT PTIME ;TIME INTO THE PRESENT SEGMENT WHEN WE SERVICE AGAIN, IN TICKS
REAL ELTIME ;ELAPSED TIME SINCE LAST READING IN TICKS
INT COUNT ;TIME ALLOWED FOR PRESENT FUNCTION
REAL ERRINT ;INTEGRAL OF THE POSITION ERROR
REAL ERRTQE ;TOTAL ERROR TORQUE TERM
REAL ERRTOL ;ERROR TOLERANCE FOR DETERMINING IF POS., VEL. WITHIN LIMITS
REAL KV ;VELOCITY ERROR FEEDBACK RATIO (NEG #)
REAL KE ;POSITION ERROR FEEDBACK RATIO (NEG #)
REAL KI ;INTEGRAL POSITION ERROR FEEDBACK RATIO (NEG #)
REAL V0 ;FRICTION RESISTANCE IN MOTOR TORQUE
REAL CI ;CURRENT JOINT GRAVITY LOADING TERM
REAL DCI ;CHANGE IN JOINT GRAVITY LOADING THROUGH SEGMENT
REAL NCI ;INITIAL GRAVITY LOADING AT START OF SEGMENT
REAL CII ;CURRENT JOINT INERTIA TERM
REAL DCII ;CHANGE IN JOINT INERTIA THROUGH SEGMENT
REAL NCII ;INITIAL JOINT INERTIA AT START OF SEGMENT
REAL FLEVEL ;FORCE LEVEL OUTPUT IF IN FORCE SERVO MODE
REAL CKI ;INTEGRAL FEEDBACK FOR COMPLIANCE SYSTEM
REAL CKV ;VELOCITY FEEDBACK FOR COMPLIANCE SYSTEM
REAL CTH0 ;ZERO POINT FOR COMPLIANT MOTION
REAL CK ;FORCE ERROR FEEDBACK COEFFICENT
REAL CKF ;GAIN IN FORCE CONTROL LOOP
INT POT ;A/D POSITION POT READING, UNMODIFIED
INT TACH ;A/D TACHOMETER READINGS, UNMODIFIED
INT POTCHN ;A/D POSITION POT CHANNEL
INT TACCHN ;A/D TACHOMETER CHANNEL, -1 IF NONE
INT DACADD ;DAC STATUS/CONTROL WORD ADDRESS
BYTE DACCHN ;DAC CHANNEL FOR MOTOR + TIME OUT RESTART
BYTE DACVAL ;OUTPUT TO DAC SAMPLE WORD
INT DACINF ;DAC TACH. FEEDBACK BIT IN LEFT HALF, BRAKE BIT IN RIGHT HALF
REAL MSCALE ;SCALE FACTOR TO CONVERT MOTOR TORQUE TO DAC UNITS
REAL TOTQE ;CONVERSION FACTOR FROM DEG. TO RAD. FOR PROPER UNITS OF TORQUE
INT DITHER ;MAGNITUDE OF DITHER TO BE ADDED TO JOINT TORQUE IN DAC UNITS
REAL SCALE ;SCALE FACTOR TO CONVERT FROM A/D UNITS TO DEGREES
REAL OFFSET ;OFFSET OF A/D POT READING
REAL USTOP ;UPPER LIMIT ON JOINT MOTION IN DEGREES
REAL LSTOP ;LOWER " " " " " "
REAL STPBND ;WIDTH OF STOP BAND BETWEEN USTOP OR LSTOP AND PHYSICAL STOP
REAL VSCALE ;TACH CONVERSION FACTOR FROM A/D UNITS TO DEGREES/TICK
INT MAXDRV ;MAXIMUM PERMITTED MOTOR DRIVE TORQUE
INT TACHZ0 ;A/D TACH READING AT ZERO VELOCITY
INT WIPERS ;POINTER TO MULTIPLE WIPER DATA, ZERO IF ONLY ONE WIPER
.IFZ ISLIN
INT NONLIN ;START OF 18 BYTES OF NON-LINEAR CALIBRATION DATA FOR POT
LSKIP 10
.ENDC
INT SPDBLK ;START OF PROCESS DESCRIPTOR BLOCK FOR THIS SERVO
;THE MULTIPLE WIPER TABLE FOLLOWS THE PROCESS DESCRIPTOR BLOCK. THE FORMAT
;FOR THIS TABLE IS LISTED ON THE PAGE WITH THE "WIPERS" SUBROUTINE.
;MECHANISM BIT FLAGS SET IN THE "MECHSM" BYTE VARIABLE
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
YELHND ==2 ;YELLOW HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND ==10 ;BLUE HAND
VISE ==20 ;VISE
SCREW ==40 ;SCREW DRIVER
;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE POLYNOMIAL
;COEFFICIENTS DATA LIST AND ASSOCIATED SERVO POINTERS:
;
; SERVO POINTERS DATA ARRAY
;
; XXXXXX TWO SERVO BIT WORDS
; XXXXXX 1 BIT FOR EACH SERVO
; BITS COMMAND MODE BITS, EG. NNUL,WOBBLE
; WOBMAG PTR TO WOBBLE MAGNITUDE CELL
; DATPT{PTR} → SEG2-. REL.POINTER TO THE NEXT SEGMENT
; TIME LENGTH OF SEGMENT IN MILLISEC
; TRANS POINTER TO LIST OF END POINT
; TRANSFORMS AND VALIDITY NUMBERS
; CODE PTR TO CODE TO BE SCHEDULED THIS SEG
; A0 F.P. POLY. COEF. FOR FIRST SERVO
; :
; A5
; :
; :
; A0
; :
; POLY{PTR}→ A5 F.P. POLY. COEF. FOR THIS SERVO
; :
; :
; A0
; :
; :
; A5
; NCI FINAL JT. GRAV. LOADING FOR 1ST JT.
; NCII FINAL JT. INERTIA FOR 1ST JOINT
; :
; FCI{PTR}→ NCI FINAL JT. GRAV. LD. FOR THIS SERVO
; NCII FINAL JT. INERTIA
; :
; SEG2: SEG3-. START OF NEXT SEGMENT
; :
; SEGN: 0 DUMMY SEGMENT, THIS SIGNALS THE
; END OF SEGMENT DATA
;
;THE NCII AND NCII TERMS ARE FILLED IN BY THE PRE-MOTION TRAJECTORY MODIFIER.
;SPACE FOR THESE TERMS MUST BE ALLOCATED BY THE CALLING INTERPRETER.
OPBITS ==4 ;RELATIVE ADDRESS OF MODE BITS, SEE PAGE 8 FOR BIT ASSIGNMENTS
WOBMG ==6 ;PTR TO WOBBLE MAGNITUDE CELL
DATST ==10 ;RELATIVE ADDRESS OF START OF SEGMENT DATA
;RELATIVE ADDRESSES OF DATA POINTED TO BY "DATPT"
;FIRST WORD CONTAINS RELATIVE POINTER TO NEXT TRAJECTORY LIST
SEGTME ==2 ;LENGTH OF TRAJECTORY SEGMENT IN TICKS
SEGTRN ==4 ;POINTS TO LIST OF END POINT TRANSFORMATIONS AND VALIDITY NUMBERS
; SEE STRUCTION OF LIST BELOW.
RNCODE ==6 ;POINTER TO CODE THAT IS TO BE SCHEDULED THIS SEGMENT
A0COEF ==10 ;CONSTANT TERM OF FIRST POLYNOMIAL
A3COEF ==24 ;A3 TERM OF FIRST POLYNOMIAL
A5COEF ==34 ;A5 " " " "
;TRANSFORM/VALIDITY LIST STRUCTURE:
;
; TRANS→ PTR TO VARIABLE 1 ;ARGUMENT GIVEN TO "GETARG"&"GETVAL"
; VALIDITY NUMBER ;# RETURNED BY TRANSFORM LAST TIME
; ; IT WAS INTERROGATED.
; PTR TO VARIABLE N
; VALIDITY NUMBER
;DEVICE BLOCK ORGANIZATION AND ASSOCIATED SERVO POINTER
;
; INUSE→ DEVICE: #NUM,0 ;NUMBER OF SERVOS STILL ACTIVE
; #EVENT ;EVENT TO BE SIGNALED WHEN ALL SERVOS DONE
; SRV01 ;LIST OF ATTACHED SERVOS
; SRV02
; :
; SRVXX
; 0 ;END OF LIST
;
;THE LOWER BYTE OF THE FIRST WORD OF THE DEVICE BLOCK IS USED TO STORE THE
;NUMBER OF SERVOS ATTACHED. THE HIGH BYTE IS USED TO SIGNAL ERROR CONDITIONS
;WHEN THE SERVOS ARE RUNNING.
; SERVO - START OF LEVEL 7 SERVO CODE
CODE
;THE ONLY ARGUMENT REQUIRED BY "SERVO" IS A POINTER TO THE SERVO BLOCK DATA
;FOR THE JOINT THAT IS TO BE SERVICED.
;
;THE "RUN" AND "MOVING" BITS IN THE SERVO STATUS WORD ARE USED TO DECIDE
;WHAT IS TO BE DONE THIS TICK. THE POSSIBLE COMBINATIONS OF RUN AND MOVING
;AND THEIR INTERPRETATIONS ARE PRESENTED BELOW:
;
; RUN MOVING INTERPRETATION
; 0 0 NO WORK TO DO, THE SERVO KILLS ITSELF.
; 0 1 STOPPING MOTION, SERVO CONTINUES TO RE-SCHEDULE ITSELF
; UNTIL THE JOINT VELOCITY IS BELOW THE ERROR TOLERANCE
; 1 0 MOTION NOT YET STARTED, SERVO CONTINUES TO RESCHEDULE ITSELF
; AND DO NOTHING UNTIL THE START WAIT COUNT {TTIME} IN
; MILLISECONDS IS LESS THAN OR EQUAL TO "PTIME".
; 1 1 JOINT RUNNING, EXECUTE SERVO CODE.
;
;THE SERVO WILL BEGIN ITS EXITING SEQUENCE IF ANY OF THE FOLLOWING EVENTS TAKES
;PLACE:
; 1. THE MOTION IS COMPLETED AND THE VELOCITY AND POSITION ERRORS HAVE BEEN
; NULLED TO WITHIN TOLERANCE.
; 2. THE VARIABLE "COUNT" HAS BEEN DECREMENTED PAST ZERO, IN WHICH CASE THE
; "TMEOUT" FLAG IS SET AND ALL OTHER SERVOS ARE SIGNALED TO STOP.
; 3. THE VARIABLE "BPANIC" IS FOUND TO BE NON-ZERO INDICATING THAT THE
; PANIC BUTTON HAS BEEN HIT.
; 4. THE SIGN BIT OF THE FIRST WORD OF THE DEVICE BLOCK IS SET INDICATING
; THAT ANOTHER SERVO HAS HAD A CATASTROPHIC ERROR OCCUR.
;
;IF AN ERROR OCCURRED WHILE THE JOINT WAS IN MOTION, THE BRAKES ARE SET AND A
;SPECIFIED AMOUNT OF TIME IS ALLOWED FOR THE JOINT VELOCITY TO GO TO ZERO
;BEFORE THE SERVO SIGNALS THE OTHER SERVOS ATTACHED TO THE DEVICE THAT IT HAS
;COMPLETED ITS MOTION.
;EXECUTION TIME: ??? MICRO SECONDS FOR COMPLETE SERVICE PERIOD
;REGISTERS USED:
;
; ALL CPU AND FLOATING POINT REGISTERS GARBAGED
;START OF LEVEL 7 CODE, CHECK IF FUNCTION HAS TIMED OUT
SERVO: SUB SDTIME(DAT),COUNT(DAT) ;DECREMENT TIME COUNT BY MIN. SAMP. INTEV.
BGT 1$ ;BRANCH IF MORE TIME LEFT
BIT #TMEOUT,(DAT) ;ARE WE STUCK IN A TIMEOUT LOOP?
BEQ .+6
BIC #MOVING,(DAT) ;YES, DONT WAIT FOR ZERO VELOCITY, JUST DIE
BIS #TMEOUT,(DAT) ;SET FUNCTION TOOK TOO LONG FLAG
JSR PC,CATERR ;SIGNAL OTHER SERVOS OF CATASTROPHIC ERROR
BR CHKSTP
;CHECK IF STOPPING OR IF ANOTHER JOINT HAS SUFFERED A FATAL ERROR
1$: BIT #RUN,(DAT) ;CHECK IF JOINT IN THE PROCESS OF STOPPING
BEQ CHKSTP ;BRANCH IF STOPPING
TST @INUSE(DAT) ;ELSE CHECK IF SOMEONE ELSE SIGNALED A CATAS. ERROR
BPL 2$ ;BRANCH IF NO ERROR SIGNALED
JSR PC,STPMVE ;ELSE SET THE BRAKES AND START EXIT SEQUENCE
BR CHKSTP
;CHECK IF THE PANIC BUTTON WAS HIT
2$: TST BPANIC ;CHECK IF THE PANIC BUTTON HAS BEEN HIT
BEQ STWAIT ;BRANCH IF OK
BIS #PANCBT,(DAT) ;ELSE INDICATE PANIC BUTTON HIT
JSR PC,CATERR ;SIGNAL OTHER JOINTS, CATASTROPHIC ERROR
;AFTER AN ERROR OCCURRED, CHECK IF JOINT MOTION STOPPED
CHKSTP: JSR PC,MOTSTP ;MAKE SURE BRAKES ARE ON
JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
BIT #MOVING,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF "MOVING" AND "RUN" ARE OFF
ABSF AC1 ;GET VELOCITY MAGNITUDE
CMPF ERRTOL(DAT),AC1 ;CHECK STOPPED, I.E. VEL LESS THAN ERR TOLERANCE
CFCC
BLT 1$ ;SKIP IF JOINT STILL MOVING
BIC #MOVING,(DAT) ;ELSE SIGNAL MOTION STOPPED
BR SRVDNE ;GO KILL SERVO
1$: JSR PC,SCHED ;STILL MOVING, RESCHEDULE TO CHECK AGAIN
JSR PC,WIPER ;CHECK IF POT WIPER IN RANGE
DISMS7 ;GO TO SLEEP
;IF START WAIT STATE, COMPARE DEAD BAND TIME "TTIME" TO PRESENT TIME "PTIME"
STWAIT: BIT #MOVING,(DAT) ;CHECK IF JOINT IN MOTION
BNE SERVNG ;IF IN MOTION, GO SERVO THE JOINT
CMP TTIME(DAT),PTIME(DAT) ;ELSE MUST BE IN START WAIT STATE,
; CHECK IF TIME TO START SERVOING
BLE 1$ ;IF PTIME≥TTIME, GO SERVO
JSR PC,SCHED ;ELSE, GO SCHEDULE THE SERVO AGAIN
DISMS7 ;GO TO SLEEP UNTIL ITS TIME TO REPEAT THE CHECK
1$: BIS #MOVING+FIRST,(DAT) ;INDICATE JOINT IN MOTION AND FIRST PASS
; THRU SERVO CODE
SUB TTIME(DAT),PTIME(DAT) ;CORRECT CURRENT TIME FOR DEAD BAND
JSR PC,BRKREL ;RELEASE THE JOINT BRAKE
BR FSTSRV
;WE ARE ACTUALLY SERVOING, CHECK IF WE NEED TO EVALUATE A POLYNOMIAL
SERVNG: LDF THN(DAT),AC0 ;UPDATE PREDICTED JOINT ANGLE
STF AC0,THP(DAT)
LDF THDN(DAT),AC0 ;UPDATE PREDICTED VELOCITY
STF AC0,THDP(DAT)
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE SAMP ;BRANCH IF SO
FSTSRV: JSR PC,SCHED ;RESCHEDULE THE JOINT
JSR PC,EVAL ;EVALATE TRAJECTORY POLYNOMIAL AND COMPUTE
; INERTIAL TORQUE
;SAMPLE THE A/D TO UPDATE THE CURRENT POSITION, "TH", AND VELOCITY, "THD",
;AND COMPUTE FEEDBACK CORRECTION
SAMP: JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
JSR PC,FEEDBK ;COMPUTE ERROR FEEDBACK TERMS
JSR PC,WIPER ;GET PROPER WIPER FOR NEXT SERVICING PERIOD
.IFNZ DIAGY
SAVDAT: MOV DBUF,EC ;SETUP DIAGNOSTIC DATA BUFFER
MOV PTIME(DAT),(EC)+ ;CURRENT TIME
TST ACTUAL ;CHECK IF ERROR READINGS TO BE SAVED
BNE SAVACT
LDF POSERR(DAT),AC0 ;SEND BACK POSITION AND VELOCITY ERROR
LDF VELERR,AC1
BR SAVRDG
SAVACT: LDF TH(DAT),AC0 ;SEND BACK ACTUAL JOINT ANGLE AND VELOCITY
LDF THD(DAT),AC1
SAVRDG: STF AC0,(EC)+
STF AC1,(EC)+
MOVB DACVAL(DAT),AC ;SAVE OUTPUT TORQUE
MOV AC,(EC)+
MOV EC,DBUF ;SAVE BUFFER POINTER FOR NEXT TIME
.ENDC
.IFNZ TIMER
MOVB CLKCNT,@TIMES ;SAVE EXECUTION TIME
INC TIMES
.ENDC
INC (DAT) ;CHANGE "NXTFIN" TO "FINAL" FLAG
BIC #NXTFIN+FIRST,(DAT)
BIT #MOVING+RUN,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF DONE
DISMS7 ;GO TO SLEEP UNTIL NEXT SERVOING PERIOD
;KILL SERVO JOB PERMENANTLY
SRVDNE: DECB @INUSE(DAT) ;CHECK IF OTHER ATTACHED SERVOS STILL ACTIVE
BGT SRVDIS ;JUST DIE IF NOT LAST ONE
ADD #2,INUSE(DAT) ;ELSE, SIGNAL TERMINATION EVENT
.IFZ SNGSTP
EVSIG @INUSE(DAT) ;SIGNAL EVENT
SRVDIS: DISMS7 ;DIE
.IFF
SRVDIS: RTS PC
.ENDC
;END OF "SERVO"
;ROUTINES FOR STOPPING JOINT MOTION
;"STPMVE" - SET BRAKES, CLEAR RUN FLAG, SET TIMEOUT COUNT, AND WAIT FOR ZERO VELO.
STPMVE: BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "STPMVE"
;"STPRUN" - STOP SERVO NOW (SETS BRAKES AND CLEARS FLAGS)
STPRUN: BIC #RUN+MOVING,(DAT) ;INDICATE KILL SERVO NOW
JSR PC,MOTSTP ;SET BRAKES AND STOP MOTOR DRIVE
RTS PC
;"CATERR" - CATASTROPHIC ERROR, SIGNAL ALL SERVOS ATTACHED TO OUR DEVICE
CATERR: BIS #100000,@INUSE(DAT) ;SIGNAL FATAL ERROR OCCURRED
BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "CATERR"
;ANGLES - CONVERTS A/D READINGS TO JOINT ANGLE AND VELOCITY
;LEAVES THE NEW JOINT ANGLE IN "TH" AND ACO, AND THE NEW ANGULAR VELOCITY
;IN "THD" AND AC1. IF THIS SERVO HAS NO TACHOMETER ( INDICATED BY TACCHN
;BEING ZERO ) OR IF THE TACH VALUE IS NEARLY OUT OF THE A/D READING RANGE,
;THE CURRENT AND PREVIOUS JOINT ANGLES ARE DIFFERENCED TO DETERMINE THE
;ANGULAR VELOCITY.
;REGISTERS USED:
;
; AC,BC,CC,DC GARBAGED
; "TH" LEFT IN AC0, "THD" LEFT IN AC1
;EXECUTION TIME: 130 MICRO SECONDS WITH TACH
; 110 MICRO SECONDS WITHOUT TACH
;COMPUTE JOINT ANGLE
ANGLES: MOVB POTCHN(DAT),ADCCHN ;START CONVERTING POT READING
LDF TH(DAT),AC1 ;GET THE PREVIOUS JOINT ANGLE
TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,AC ;GET POT READING
MOVB TACCHN(DAT),ADCCHN ;START CONVERTING TACH READING
MOV AC,POT(DAT)
ADD #4000,AC ;SHIFT POT READING TO 0 → 7777
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT
.IFZ ISLIN
MOV AC,DC ;SAVE NORMALIZED POT READING
CLR BC
ASHC #-10,AC ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR BC ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD DAT,AC ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(AC),CC ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(AC),AC ;GET UPPER LIMIT TO INTERPOLATE
SUB CC,AC ;GET DIFFERENCE
MUL BC,AC ;INTERPOLATE
ASHC #1,AC ;GET 16 MSB
ADD CC,AC ;NOW HAVE PROPER CALIBRATION
ADD DC,AC ;ADD TO NORMALIZED POT READING
.ENDC
;CONVERT TO JOINT ANGLE
LDCIF AC,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(DAT),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(DAT),AC0 ;ADD POT OFFSET
STF AC0,TH(DAT) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
;COMPUTE THE ANGULAR VELOCITY OF THE JOINT
.IFZ TACCAL
TST TACCHN(DAT) ;CHECK IF THERE IS A A/D TACH CHANNEL
BGT 2$ ;JUMP IF WE DO HAVE A TACHOMETER
1$: SUBF AC0,AC1 ;NO TACH, DIFFERENCE POSITION READINGS
NEGF AC1
DIVF ELTIME(DAT),AC1 ;DIVID BY ELAPSED TIME BETWEEN READINGS
STF AC1,THD(DAT) ;SAVE VELOCITY( DEG/TICK )
RTS PC ;RETURN
2$: TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
SUB TACHZ0(DAT),AC ;SUBTRACT ZERO VELOCITY TACH READINGS
MOV AC,BC
BPL .+4
NEG BC
CMP #3400,BC ;CHECK IF READING OUT OF RANGE
BLE 1$ ;DIFFERENCE POSITION IF SO
BIC #7,BC ;VEL = 0 IF IN NOISE BAND
BNE .+4
CLR AC
LDCIF AC,AC1 ;CONVERT TO FLOATING POINT
MULF VSCALE(DAT),AC1 ;CONVERT FROM A/D UNITS TO DEGREES/TICK
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
.IFF
TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
LDCIF AC,AC1 ;ACCUMULATE TACH READINGS
TST AC ;GET ABS VALUE OF READING
BGE .+4
NEG AC
ADDF TTACH,AC1
CMP #3400,AC ;CHECK IF READING OUT OF RANGE
BGT .+6
INC TACOFR ;KEEP TRACK OF NUMBER OF READINGS OUT OF RANGE
STF AC1,TTACH
INC TACRDG ;KEEP TRACK OF NUMBER OF READINGS TAKEN
CLRF AC1 ;SET ZERO VELOCITY
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
DATA
TTACH: .BLKW 2 ;ACCUMULATED TACH READINGS FOR TACCAL MODE
TACRDG: 0 ;NUMBER OF TACH READINGS TAKEN
TACOFR: 0 ;NUMBER OF TACH READINGS OUT OF RANGE
CODE
.ENDC
;END OF "ANGLES"
;FEEDBK - SERVO FEED BACK LOOP TO DRIVE THE JOINT MOTOR
;COMPUTES THE OUTPUT MOTOR DRIVE BY DETERMINING THE POSITION AND VELOCITY ERROR
;AND ADDING IN THE EFFECTS OF GRAVITY LOADING, FRICTION, AND INERTIAL ACCELERATION.
;THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; MOTOR TORQUE = CII*THDDP + CI - KE*(TH-THP) - KV*CII*(THD-THDP)
; - (KI/CII)*INTEGRAL(TH-THP) + or - V0
; WHERE
; CII = JOINT INERTIA
; CI = JOINT GRAVITY LOADING
; TH = ACTUAL JOINT ANGLE
; THP = PREDICTED JOINT ANGLE
; THD = ACTUAL JOINT ANGULAR VELOCITY
; THDP = PREDICTED JOINT ANGULAR VELOCITY
; THDDP= PREDICTED JOINT ANGULAR ACCELERATION
; V0 = FRICTION COMPENSATION WITH THE SAME SIGN AS VELOCITY
; KE,KI,KV = FEEDBACK COEFFICIENTS
;
;THE INTEGRAL OF POSITION ERROR IS DEVELOPED BY SUMMING THE POSITION ERROR EACH
;TIME THE SERVO IS SERVICED.
;EXECUTION TIME: 240 MICRO SECONDS
;REGISTERS USED:
; AC0 MUST BE LOADED WITH "TH" AND AC1 "THD" PRIOR TO CALLING FEEDBK
; AND THE INERTAL TORQUE MUST BE LOADED INTO AC3 IF ON TRAJECTORY
FEEDBK: SUBF THP(DAT),AC0 ;SUBT THE PREDICTED JOINT ANGLE FROM THE ACTUAL
BIT #FINAL,(DAT) ;CHECK IF MOTION COMPLETED
BEQ CMPFBK ;BRANCH IF STILL ON TRAJECTORY
;AT END OF TRAJECTORY, NULL ERRORS AND EXIT IF WITHIN TOLERANCE
BIC #FSERVO,(DAT) ;DON'T FORCE SERVO DURING NULLING TIME
CLRF AC3 ;IGNOR JOINT INERTIAL ACCELERATION
CLRF THDP(DAT) ;PREDICTED VELOCITY MUST BE ZERO
BIT #NNUL,MODBTS(DAT);CHECK IF WE NULL ERRORS AT END OF TRAJ.
BEQ 1$
JSR PC,STPMVE ;NO NULLING, GO STOP MOVING
JSR PC,MOTSTP
BR 2$
1$: STF AC0,AC2 ;NULLING, CHECK IF WITHIN FINAL TOLERANCE
ABSF AC2 ;GET ABS( POSITION ERROR )
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED TOLERANCE
CFCC
BLT ENDTRJ ;IF ERROR TOO LARGE, USE FEEDBACK TO REDUCE IT
2$: STF AC1,AC2 ;GET ABS( VELOCITY )
ABSF AC2
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED VELOCITY TOLERANCE
CFCC
BLT ENDTRJ ;IF TOO LARGE, CONTINUE SERVOING TO SET POINT
JSR PC,STPRUN ;NOW WITHIN TOLERANCE, KILL SERVO
RTS PC
ENDTRJ: JSR PC,SCHED ;RESCHEDULE JOINT TO RUN AGAIN
;COMPUTE FEEDBACK ERROR TERMS AND CORRECT MOTOR DRIVE
CMPFBK: STF AC0,POSERR(DAT) ;SAVE THE POSITION ERROR
BIT #FSERVO,(DAT) ;FORCE SERVOING?
BEQ 1$
LDF FLEVEL(DAT),AC0 ;YES, GET COMPLIANCE LEVEL
STF AC0,ERRTQE(DAT)
BR 2$ ;JUST ADD GRAVITY LOADING AND FRICTION COMP.
1$: STF AC0,AC2
.IFZ TACCAL
SUBF THDP(DAT),AC1 ;SUBT THE PREDICTED ANGULAR VELOCITY FROM THE ACTUAL
.ENDC
.IFNZ DIAGY
STF AC1,VELERR ;SAVE THE VELOCITY ERROR
.ENDC
ADDF ERRINT(DAT),AC2 ;SUM THE INTEGRAL POSITION ERROR
STF AC2,ERRINT(DAT)
MULF KI(DAT),AC2 ;MULTIPLY INTEGRAL BY FEEDBACK COEF., KI
DIVF CII(DAT),AC2 ;FOR STABILITY, DIVID KI BY JOINT INERTIA
MULF KE(DAT),AC0 ;MULT THE POSITION ERROR BY FEEDBACK COEF.,KE
ADDF AC2,AC0 ;POSITION + INTEGRAL ERROR FEEDBACK
MULF KV(DAT),AC1 ;MULTIPLY VELOCITY ERROR BY FEEDBACK COEF., KV
MULF CII(DAT),AC1 ;FOR STABILITY, MULT KV BY JOINT INERTIA
ADDF AC1,AC0 ;TOTAL TORQUE FEEDBACK
MULF TOTQE(DAT),AC0 ;CONVERT TO UNITS OF TORQUE
STF AC0,ERRTQE(DAT) ;SAVE THE JOINT ERROR TORQUE
ADDF AC3,AC0 ;ADD THE INERTIAL TORQUE
2$: ADDF CI(DAT),AC0 ;ADD GRAVITY LOADING
;ADD IN THE EFFECTS OF THE PREDICTED FRICTIONAL RESISTANCE
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE 3$ ;DON'T LOOK AT VELOCITY IF SO
MOV THD(DAT),AC ;GET THE SIGN AND THE MSB OF ACTUAL JOINT VELOCITY
BNE 4$
BIT #FSERVO,(DAT) ;IF FORCE SERVOING POSERR INVALID
BNE 6$
3$: MOV POSERR(DAT),AC ;USE THE PREDICTED TH - ACTUAL TH IF VELOCITY ZERO
BEQ 6$ ;DONT ADD ANY FRICTION IF BOTH VEL AND POS ERROR ZERO
NEG AC
4$: BLT 5$ ;FRICTION RESISTS MOVEMENT, CHECK DIRECTION OF VEL.
ADDF V0(DAT),AC0 ;POS. VELOCITY, COMPENSATE FOR FRICT. BY ADDING V0
BR 6$
5$: SUBF V0(DAT),AC0 ;NEG. VELOCITY, " " " " SUBT. V0
6$: JSR PC,MOTDRV ;DRIVE MOTOR
RTS PC ;RETURN
;END OF "FEEDBK"
;SCHED - SCHEDULES SERVO TO RUN AGAIN
;RESCHEDULES THE SERVO WITHIN A MINIMUM OF "SDTIME" TICKS. THE FOLLOWING
;VARIABLES ARE ALSO UPDATED:
;
; PTIME ← PTIME + ELAPSED TIME TILL NEXT RUN
; ELTIME ← FLOAT( ELAPSED TIME TILL NEXT RUN )
;
;MULTIPLE SCHEDULING CALLS ARE NOT CHECKED FOR.
;EXECUTION TIME:
;REGISTERS USED:
; AC,BC,CC,DC,EC GARBAGED
; LEAVES VALUE OF ELTIME IN AC2
SCHED:
.IFZ SNGSTP
RESCH7 SDTIME(DAT) ;RE-SCHEDULE JOINT SERVO
LDCIF (SP),AC2 ;GET ACTUAL TIME TILL NEXT RUN
ADD (SP)+,PTIME(DAT);KEEP TRACK OF TIME INTO PRESENT SEGMENT
.IFF
LDCIF SDTIME(DAT),AC2 ;IF IN DIAGNOSTIC MODE, DONT SCHEDULE
ADD SDTIME(DAT),PTIME(DAT)
.ENDC
STF AC2,ELTIME(DAT) ;SAVE F.P. ELAPSED TIME
RTS PC ;RETURN
;END OF "SCHED"
;EVAL - FETCHES POLYNOMIAL COEF. AND EVALUATES FOR NEXT SERVICING TIME
;THE JOINT ACCELERATION IS COMPUTED BY DETERMINING THE CONSTANT ACCELERATION
;THAT WILL PRODUCE THE SAME CHANGE IN ANGLE BETWEEN NOW AND THE NEXT SERVICING
;PERIOD AS THE 5TH ORDER POLYNOMIAL. THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; ACCELERATION = ( V2 - V1 )/DT
; V2 = ( P2 - P1 )/DT
; P2 = P2' + DELTHD*DT + WOBBLE
;
; WHERE P1 = PREDICTED JOINT ANGLE FOR CURRENT SERVICING PERIOD
; V1 = " " VELOCITY " " " "
; P2 = " " ANGLE FOR THE NEXT SCHEDULED PERIOD
; P2' = POLYNOMIAL JOINT " " " " " "
; DELTHD= VELOCITY DEVIATION OF RUN TIME PATH FROM POLY. VALUE
; DT = INTERVAL OF TIME UNTIL WE ARE SCHEDULED TO BE SERVICED
; WOBBLE= SINUSOIDAL WOBBLE IN JTS 4,5,6
;
;THE PREDICTED JOINT POSITION AND VELOCITY FOR THE NEXT SERVICING PERIOD ARE
;STORED IN "THN(DAT)" AND "THDN(DAT)" AND THE INERTIAL TORQUE IS LEFT IN AC3
;
;ALSO, THE CURRENT JOINT INERTIA AND GRAVITY LOADING IS DETERMINED BY
;INTERPOLATION. THE COMPUTED VALUES ARE SAVED IN CII AND CI.
;EXECUTION TIME: 320 MICRO SEC. WHILE CHANGING SEGMENTS
; 310 MICRO SEC. FOR IN MIDDLE OF SEGMENT
; +100 MICRO SEC. IF JOINT OUT OF RANGE
;REGISTERS USED:
; EXPECTS VALUE OF ELTIME IN AC2
; LEAVES INERTIAL TORQUE IN AC3
EVAL: MOV DATPT(DAT),CC ;GET POINTER TO START OF POLY. DATA LIST
BNE 1$
;NOT ON POLYNOMIAL, JUST COMPUTE CHANGE DUE TO CONSTANT VELOCITY FACTOR
CLRF AC0 ;ZERO POLYNOMIAL POSITION
LDF CII(DAT),AC3 ;USE CONSTANT JOINT INERTIA
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;ON POLYNOMIAL, CHECK IF END OF SEGMENT OR START OF FIRST SEGMENT
1$: MOV TTIME(DAT),AC ;GET PRESENT TOTAL SEGMENT TIME
BIT #FIRST,(DAT) ;ARE WE JUST STARTING A MOTION?
BNE FSTSEG ;GO SET FIRST SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST END OF SEGMENT
BLT EVALPY ;BRANCH IF STILL IN SAME SEGMENT
;SWITCHING TO A NEW SEGMENT SAVE DYNAMIC COEF. AND CHECK IF FINAL SEGMENT
CHKFIN: MOV FCI(DAT),BC ;GET POINTER TO GRAV. LDG. AND INERTIA
MOV (BC)+,NCI(DAT) ;FINAL GRAV. LDG. OF PAST SEG= INITIAL GRAV. LDG
MOV (BC)+,NCI+2(DAT); OF NEXT SEGMENT
MOV (BC)+,NCII(DAT) ;SAME FOR JOINT INERTIA
MOV (BC),NCII+2(DAT)
MOV (CC),BC ;SAVE INCREMENT TO NEXT SEGMENT
ADD BC,CC ;GET THE POINTER TO THE START OF THE NEXT SEGMENT
TST (CC) ;CHECK IF FINAL SEGMENT
BNE 1$ ;BRANCH IF NOT
;FINISHED MOTION, COMPUTE FINAL POSITION AND DYNAMIC COEF.
MOV POLY(DAT),CC ;GET POINTER TO POLYNOMIAL LIST
LDF (CC),AC0 ;COMPUTE FINAL SET POINT FROM POLY, GET A5
ADDF -(CC),AC0 ; + A4
ADDF -(CC),AC0 ; + A3
ADDF -(CC),AC0 ; + A2
ADDF -(CC),AC0 ; + A1
ADDF -(CC),AC0 ; + A0
MOV FCI(DAT),CC ;GET PTR TO GRAV. LDG. AND INERTIA
LDF 4(CC),AC3 ;SET FINAL VALUE OF JOINT INERTIA
MOV (CC)+,CI(DAT) ;SET FINAL VALUE OF GRAVITY LOADING
MOV (CC),CI+2(DAT)
STF AC3,CII(DAT)
CLR DATPT(DAT) ;INDICATE NO MORE POLYNOMIALS
BIS #NXTFIN,(DAT) ;INDICATE NEXT SERVICING PERIOD IS LAST
MOV #NULTME,COUNT(DAT) ;ALLOW NULTME SECONDS TO NULL ERRORS
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;NOT THE FINAL SEGMENT, UPDATE POINTERS, DYNAMIC COEF, AND SEG. TIME
1$: MOV CC,DATPT(DAT) ;SAVE NEW SEGMENT POINTER
ADD BC,FCI(DAT) ;UPDATE OTHER DATA POINTERS
ADD BC,POLY(DAT)
SUB AC,PTIME(DAT) ;GET TIME INTO NEXT SEGMENT
FSTSEG: MOV SEGTME(CC),AC ;SET THE TOTAL TIME OF THE NEXT SEG
LDCIF AC,AC0 ;FLOAT THE TOTAL SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST THIS NEW SEGMENT
BGE CHKFIN
MOV AC,TTIME(DAT) ;SAVE THE NEW TOTAL SEGMENT TIME
STF AC0,FTTIME(DAT) ;SAVE THE F.P. TOTAL TIME
MOV FCI(DAT),AC ;COMPUTE CHANGE IN GRAVITY LOADING
LDF (AC)+,AC3
SUBF NCI(DAT),AC3
STF AC3,DCI(DAT)
LDF (AC),AC3 ;COMPUTE CHANGE IN INERTA
SUBF NCII(DAT),AC3
STF AC3,DCII(DAT)
;EVALUATE THE POSITION POLYNOMIAL FOR THE NEXT SERVICING PERIOD.
EVALPY: LDCIF PTIME(DAT),AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
MOV POLY(DAT),CC ;GET ABSOLUTE ADDRESS OF POSITION POLYNOMIAL
DIVF FTTIME(DAT),AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
LDF (CC),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
;INTERPOLATE GRAVITY LOADING AND JOINT INERTIA
LDF DCI(DAT),AC3 ;GET CHANGE IN GRAVITY LOADING
MULF AC1,AC3 ; x TIME INTO SEGMENT
ADDF NCI(DAT),AC3 ; + INITIAL G.L. FOR SEGMENT
STF AC3,CI(DAT) ;SAVE NEW GRAVITY LOADING
LDF DCII(DAT),AC3 ;REPEAT FOR JOINT INERTIA
MULF AC1,AC3
ADDF NCII(DAT),AC3
STF AC3,CII(DAT) ;SAVE NEW JOINT INERTIA
;ADD IN A CONSTANT VELOCITY FACTOR TO MODIFY THE TRAJECTORY WHILE RUNNING
NUDGE: LDF DELTHD(DAT),AC1 ;LOAD THE VELOCITY FACTOR
MULF AC2,AC1 ;VELOCITY * ELAPED TIME = CHANGE IN POSITION
ADDF DELTH(DAT),AC1 ;DETERMINE THE TOTAL POSITION DEVIATION
ADDF AC1,AC0 ;MODIFY NEXT POSITION
STF AC1,DELTH(DAT) ;SAVE THE TOTAL POSITION DEVIATION
;ADD IN WOBBLE IN ANGLE IN LAST 3 JOINTS
BIT #WOBBLE,MODBTS(DAT) ;WOBBLING?
BEQ NOWOB ;NO
MOV WOBPTR(DAT),AC ;INDEX TO SINE TABLE
BLT NOWOB ;ONLY WOBBLE JTS 4,5,6
LDF WOBTAB(AC),AC1 ;COMPUTE WOBBLE SINUSOID
MULF @WOBMAG(DAT),AC1
CMP (AC)+,(AC)+ ;INDEX AND SAVE PTR
BIC #177700,AC
MOV AC,WOBPTR(DAT)
ADDF AC1,AC0 ;ADD WOBBLE TO NEW POSITION
;CHECK IF JOINT GOING BEYOND STOP LIMIT
NOWOB: BIC #PASLIM,(DAT) ;ASSUME JOINT IN RANGE
CMPF USTOP(DAT),AC0 ;COMPARE TO UPPER STOP LIMIT
CFCC
BGE 2$ ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 1$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BGE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
1$: SUBF USTOP(DAT),AC0 ;COMPUTE DISTANCE BEYOND STOP LIMIT
STF AC0,AC1
ADDF STPBND(DAT),AC0 ;(TH-STOP)+(WIDTH OF STOP BAND)
DIVF AC0,AC1 ;PERCENTAGE INTO THE STOP BAND
MULF STPBND(DAT),AC1 ;NEW ANGLE BEYOND STOP LIMIT
LDF USTOP(DAT),AC0 ;NEW ANGLE IS STOP LIMIT+DISTANCE INTO STOP BAND
ADDF AC1,AC0
BR SVENTH
2$: CMPF LSTOP(DAT),AC0 ;COMPARE TO LOWER STOP LIMIT
CFCC
BLE JTINRG ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 3$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BLE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
3$: SUBF LSTOP(DAT),AC0 ;SAME PROCESS AS BEYOND UPPER LIMIT
STF AC0,AC1
SUBF STPBND(DAT),AC0
DIVF AC0,AC1
MULF STPBND(DAT),AC1
LDF LSTOP(DAT),AC0
SUBF AC1,AC0
JTINRG: BIC #STROUT,(DAT) ;INDICATE JOINT NOT STARTED OUT OF RANGE
;COMPUTE JOINT INERTIAL ACCELERATION AND TORQUE
SVENTH: STF AC0,THN(DAT) ;SAVE THE PREDICTED JOINT ANGLE FOR NEXT TIME
SUBF THP(DAT),AC0 ;SUBT CURRENT JOINT ANGLE FROM NEXT PREDICTED ANGLE
DIVF AC2,AC0 ;DIVID BY ELAPED TIME
STF AC0,THDN(DAT) ;SAVE THE PREDICTED VELOCITY FOR NEXT SERVICING TIME
SUBF THDP(DAT),AC0 ;SUBT CURRENT PREDICTED VELOCITY
DIVF AC2,AC0 ;DIVID BY TIME AGAIN
MULF TOTQE(DAT),AC3 ;CONVERT DEGREES TO RADIANS IF NECESSARY
MULF AC0,AC3 ;MULT BY JOINT INERTIA AND LEAVE VALUE OF
; NEW INERTIAL TORQUE IN AC3
RTS PC ;RETURN
;END OF "EVAL"
;WIPER - SWITCHES A/D CHANNELS IF A/D OUT OF RANGE AND SERVO HAS MULTIPLE WIPERS
;OR'S A 1 INTO "OUTRGE" IF A WIPER WAS SWITCHED. ASSUMES "WIPERS" POINTS TO
;A LIST OF DATA ARRANGED AS FOLLOWS:
;
; .BLKW 0,0,0,0,0 ;BOTTOM END OF LIST
; :
; A/D CHAN # ;ONE WORD INCREASING READING
; F.P. OFFSET ;TWO WORDS ↓
; F.P. SCALE ;TWO WORDS
; WIPERS → A/D CHAN # ;ONE WORD
; F.P. OFFSET ;TWO WORDS
; F.P. SCALE ;TWO WORDS
; :
; .BLKW 0 ;TOP END OF LIST
;REGISTERS USED:
;
; AC GARBAGED, DAT REFERENCED
WIPER: MOV WIPERS(DAT),AC ;GET THE POINTER TO THE WIPER DATA
BNE .+4
RTS PC ;RETURN IF SERVO HAS ONLY ONE WIPER
CMP POT(DAT),#-2400 ;CHECK IF A/D READING OUT OF RANGE
BGT 2$ ;JUMP IF NOT TOO LOW
SUB #12,AC ;TOO LOW- POINT TO PREVIOUS WIPER DATA
1$: TST (AC) ;CHECK IF AT END OF WIPER LIST
BNE .+4
RTS PC ;RETURN IF WE CAN'T SWITCH TO A NEW WIPER
MOV AC,WIPERS(DAT) ;ELSE SAVE THE POINTER TO THE NEW WIPER DATA
MOV (AC)+,POTCHN(DAT) ;SAVE THE NEW A/D CHANNEL NUMBER
MOV (AC)+,OFFSET(DAT) ;SAVE THE NEW F.P. OFFSET VALUE
MOV (AC)+,OFFSET+2(DAT)
MOV (AC)+,SCALE(DAT) ;SAVE NEW F.P. SCALE FACTOR
MOV (AC),SCALE+2(DAT)
BIS #1,OUTRGE ;INDICATE WIPER OUT OF RANGE
RTS PC ;RETURN
2$: CMP POT(DAT),#2400 ;CHECK IF A/D READING TOO HIGH
BGE .+4
RTS PC ;RETURN IF WITHIN RANGE
ADD #12,AC ;ELSE, POINT TO THE NEXT WIPER DATA
BR 1$ ;GO SWITCH WIPERS
DATA
OUTRGE: 0 ;"WIPER" OR'S A 1 INTO THIS WORD IF POT WIPER OUT OF RANGE,
; NEVER RESET BY SERVO CODE.
CODE
;END OF "WIPER"
;MOTDRV,MOTSTP,BRKREL - MOTOR DRIVE ROUTINES
;"MOTDRV" SETS THE MOTOR DRIVE FOR CURRENT SERVO. THE OUTPUT TORQUE MUST BE
;LOADED INTO AC0 BEFORE THE CALL TO "MOTDRV". IF THE DRIVE REQUESTED IS TOO
;LARGE, "EXJTFC" IS SET IN THE STATUS WORD AND ALL SERVOS ATTACHED TO THE SAME
;DEVICE AS THIS JOINT ARE STOPPED. THE FINAL DESIRED MOTOR TORQUE IS THE
;SUM OF THE VALUE PLACED IN AC0 BEFORE THIS ROUTINE WAS CALLED AND A DITHER
;TERM.
;EXECUTION TIME: 40 MICRO SEC. IF NO CATASTROPHIC ERROR
MOTDRV: MULF MSCALE(DAT),AC0 ;CONVERT THE MOTOR TORQUE TO A/D UNITS
NEG DITHER(DAT) ;COMPLEMENT THE JOINT DITHER TERM
STCFI AC0,CC ;CONVERT THE MOTOR DRIVE TO INTEGER
ADD DITHER(DAT),CC ;ADD DITHER TO JOINT TORQUE
MOV CC,DC ;GET ABSOLUTE VALUE
BGE .+4
NEG DC
CMP MAXDRV(DAT),DC ;CHECK IF DRIVE TOO LARGE
BGE 1$ ;BRANCH IF WITHIN RANGE
BIS #EXJTFC,(DAT) ;INDICATE EXCESSIVE JOINT FORCE
JSR PC,CATERR ;CATASTROPHIC ERROR, GO STOP ALL RELATED SERVOS
BR MOTSTP ;STOP MOTION OF THIS JOINT
1$: MOVB CC,DACVAL(DAT) ;SET DAC OUTPUT VALUE
.IFZ SNGSTP
MOV DACCHN(DAT),@DACADD(DAT) ;SEND TO DAC
.ENDC
RTS PC ;RETURN
;"BRKREL" RELEASES THE BRAKE FOR THE MOTOR BY CLEARING THE BRAKE BIT IN THE DAC REG
;EXECUTION TIME:
BRKREL: MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
.IFZ SNGSTP
BISB DACINF(DAT),2(AC) ;CLEAR THE BRAKE BIT
.ENDC
RTS PC ;RETURN
;"MOTSTP" RESETS THE MOTOR DRIVE AND SETS THE BRAKE
;EXECUTION TIME:
MOTSTP: MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
CLRB DACVAL(DAT) ;ZERO DAC OUTPUT
.IFZ SNGSTP
MOV DACCHN(DAT),(AC)+ ;SET A ZERO MOTOR DRIVE
BICB DACINF(DAT),(AC) ;SET THE BRAKE
.ENDC
RTS PC
;END OF MOTOR DRIVE FUNCTIONS
; INTARM - INITIALIZES SERVO CODE FOR FUTURE ARM MOTIONS
;THIS ROUTINE SHOULD BE CALLED AT LEAST ONCE BEFORE ANY ARM MOTIONS ARE
;INITIATED. THE FOLLOWING OPERATIONS ARE PERFORMED:
;
; 1) THE PANIC BUTTON INTERRUPT VECTOR IS SETUP TO SET THE VARIABLE
; "BPANIC" NON-ZERO WHENEVER THE PANIC BUTTON IS HIT WHILE ENABLED.
; 2) THE REFERENCE POWER SUPPLY VOLTAGE IS READ AND ITS VALUE IS
; COMPARED TO ITS VALUE AT THE TIME THE ARM WAS CALIBRATED. IF A
; SIGNIFICANT DISCREPENCY EXISTS, A ERROR CODE IS RETURNED.
; 3) A ZERO VELOCITY TACHOMETER READING IS TAKEN FOR ALL OPERATIONAL
; JOINTS WITH TACHS. THE ZERO READING IS STORED IN THE "TACHZ0"
; WORD OF THE SERVO DATA BLOCKS. IF A READING DEVIATES BY MORE
; THAN 10 ADC COUNTS FROM THE INITIAL "TACHZ0" VALUE, AN ERROR
; IS SIGNALED.
; 4) THE CURRENT JOINT ANGLE FOR ALL OPERATIONAL JOINTS IS READ AND
; SAVED IN THE SERVO DATA BLOCKS. AT THE SAME TIME, THE GRAVITY
; LOADING AND INERTIA CONSTANT FOR EACH JOINT IS COMPUTED.
; 5) THE SERVO DATA BLOCKS FOR ALL OPERATIONAL JOINTS ARE INITIALIZED.
; 6) FORCE EVENT LIST IS INITIALIZED AND ALL FORCE SENSING IS
; TERMINATED.
;
;THIS ROUTINE REQUIRES AS ITS ONLY ARGUMENT THE ADDRESS OF A DEVICE BLOCK WHICH
;IS AT LEAST 2+N WORDS LONG, WHERE N IS THE NUMBER OF JOINTS CURRENTLY IN
;EXISTANCE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;DEVICE STORAGE BLOCK
; JSR PC,INTARM
; TST R0 ;CHECK FOR ERROR CONDITIONS
;
;AT THE END OF EXECUTION, REGISTER R0 RETURNS THE ERROR CODE GENERATED BY THIS
;ROUTINE. THE INTERPRETATION OF THIS ERROR CODE IS GIVEN ON PAGE 8 OF THIS
;PROGRAM.
;REGISTERS USED:
; R0 AND R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
INTARM: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;SETUP BUTTON INTERRUPT HANDLER ROUTINE
.IFZ SNGSTP
SETVEC #ARMTRP,#ARMERR,#SUPST7 ;SET PANIC BUTTON TRAP ROUTINE
SETVEC #DRBTRP,#YERR,#SUPST7 ;SET YELLOW PANIC BUTTON TRAP ROUTINE
CLR KICDAC ;INDICATE NO DAC REFRESH JOB RUNNING
.ENDC
;RANDOM INITIALIZATION
CLR ADCSR ;TURN OFF ADC INTERRUPTS
; MOV #1,DR11S ;ENABLE YELLOW PANIC INTERUPT
; MOV #4000,DR11O
CLR ACTNUM ;RESET TOUCH SENSOR EVENT DATA
MOV #TCHVAL,R3
MOV #LSTUSE-TCHVAL/2,R4
CLR (R3)+
SOB R4,.-2
;INITIALIZE FORCE SENSING/COMPLIANCE ROUTINES
MOV #FRCJOB,R2 ;FREE ALL FORCE JOB BLOCKS
MOV R2,FRCPTR ;FORM ONE BIG CHAIN OF LINKED BLOCKS
MOV #FRCBLK,R4 ;NUMBER OF BLOCKS TO LINK
1$: MOV R2,R3 ;LINK THEM
ADD #16,R2 ;EACH BLOCK IS 7 WORDS LONG
MOV R2,(R3)
SOB R4,1$
CLR (R3) ;ZERO MARKS END
MOV #FZAPS,R2 ;ZERO ALL REQUIRED FORCE WORDS
MOV #FZAPE-FZAPS/2,R3
CLR (R2)+
SOB R3,.-2
;CLEAR THE LOCKS ON NON-REENTRANT CODE
MOV #-1,STTLCK ;SETTH
MOV #-1,SRFLCK ;SETREF
MOV #-1,BASLCK ;SETBAS
MOV #-1,WSTLCK ;WRIST
CLR BOVLCK ;BOVERM LOCK
;CHECK WHICH JOINT SERVOS ARE OPERATIONAL
MOV #MAXSRV,R3 ;GET NUMBER OF EXISTING SERVOS TO CHECK
MOV #SERVOS,R0 ;GET PTR TO LIST OF EXISTING SERVOS
CLR R4 ;CONSTRUCT SERVO BITS IN HERE
CLR R5
2$: MOV (R0)+,R2 ;GET POINTER TO SERVO DATA BLOCK
ASHC #1,R4 ;MAKE ROOM FOR NEXT SERVO BIT
BIT #NONEX,(R2) ;CHECK IF JOINT OPERATIONAL
BNE 3$ ;BRANCH IF ITS NOT
BIS #1,R5 ;ELSE SET SERVO BIT FOR FURTHER CHECKING
JSR PC,SETSET ;INITIALIZE SERVO DATA VARIABLES
3$: SOB R3,2$ ;REPEAT FOR ALL EXISTING SERVOS
ASHC #32.-MAXSRV,R4 ;LEFT JUSTIFY SERVO BITS
MOV R4,INCOEF ;SAVE SERVO BITS IN LOCAL COEFFICENT LIST
MOV R5,INCOEF+2
;ATTACH REQUIRED SERVOS
MOV #INTBLK,R0 ;POINT TO PTR TO SERVO BIT LIST
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE AND SET UP
; DEVICE BLOCK
BCS INTDNE ;BRANCH IF ATTACH ERROR OCCURRED
TST R0 ;BRANCH IF AT LEAST ONE SERVO EXISTS
BGT 4$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
BR INTDNE
;READ CURRENT JOINT ANGLES, TACH ZERO, AND REFERENCE POWER SUPPLY
4$: JSR PC,SETREF ;CHECK REFERENCE POWER SUPPLY VOLTAGE AND
; ZERO VELOCITY TACH READINGS
BCS INTDNE ;BRANCH IF ANY ERROR OCCURRED
JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL JOINTS
BCS INTDNE ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO ERRORS
;EXIT SEQUENCE
INTDNE: JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE THE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;RETURN
;INFORMATION FOR SERVO DATA BLOCKS
DATA
INTBLK: INCOEF ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
0
0
;LOCAL STORAGE
BOVLCK: .WORD 0 ;FLAG TO SIGNAL OVERRUN CONDITION
INCOEF: .WORD 0,0,0 ;SHORT COEFFICENT LIST USED TO ATTACH SERVOS
CODE
;END OF "INTARM"
;DRIVE - INITIATES MOTIONS FOR ONE JOINT, SINGLE SEGMENT
;A DIFFERENTIAL CHANGE IN JOINT ANGLE IS INITIATED FOR A SINGLE SERVO. THIS
;MODE IS INTENDED PRIMARY AS A DIAGNOSTIC TOOL TO CALIBRATE THE JOINT VARIABLES.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 4 WDS.
; JSR PC,DRIVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
.IFNZ DIAGY
DRIVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
;ATTACH REQUIRED SERVO
MOV R0,DRVBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV R0,R4
MOV #DRVBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS DRVERR ;BRANCH IF ATTACH ERROR OCCURRED
CMP #1,R0 ;CHECK THAT ONLY ONE JOINT HAS BEEN SELECTED
BEQ 1$ ;BRANCH IF ONLY ONE
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT
;CORRECT TRAJECTORY FOR INITIAL JOINT ANGLE
1$: ADD #DATST+A0COEF,R4;POINT TO CONSTANT TERM OF FIRST POLYNOMIAL
MOV 4(R1),R2 ;GET POINTER TO JOINT THAT IS TO BE SERVOED
MOV TH(R2),(R4) ;SET INITIAL VALUE OF POLY TO PRESENT JOINT ANGLE
MOV TH+2(R2),2(R4)
;CHECK IF FINAL JOINT ANGLE OUT OF RANGE
MOV #5,R3 ;ADD ALL SIX COEFFICIENTS
LDF (R4)+,AC0 ;LOAD CONSTANT TERM
ADDF (R4)+,AC0 ;ADD IN ALL OTHER COEFFICIENTS
SOB R3,.-2
CMPF USTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT UPPER STOP LIMIT
CFCC
BGE 3$ ;SKIP IF WITHIN RANGE
2$: MOV #4,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT CLEANLY
3$: CMPF LSTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT LOWER STOP LIMIT
CFCC
BGT 2$ ;BRANCH IF NOT IN RANGE
;INITIATE SERVOS TO RUN
.IFNZ DIAGY
MOV #DBUF+2,DBUF ;SETUP DIAGNOSTIC BUFFER
.ENDC
.IFNZ TACCAL
CLR TTACH ;CLEAR ACCUMULATED TACH READING IF TACCAL MODE
CLR TTACH+2
CLR TACRDG ;CLEAR TOTAL NUMBER OF TACH READINGS TAKEN
CLR TACOFR ;ZERO TOTAL NUMBER OF READINGS OUT OF RANGE
MOV #BELL,SG
JSR PC,TYPSTR
.ENDC
MOV DRVBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV ;GO RUN THE SERVOS
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
DRVERR: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
DRVBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
48. ;DELAY STARTING BY 48 MILLISECONDS
DATST ;POINTER TO START O F SEGMENT DATA
CODE
.ENDC
;END OF "DRIVE"
;CENTER - CLOSES THE HAND FINGERS AND CENTERS THE ARM OVER ANY GRASPED OBJECT
;THE ORGANIZATION OF THE COEFFICIENT DATA ARRAY MUST BE AS FOLLOWS:
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
; XXXXXX ; SERVO AND ALL JOINT SERVOS OF THE SAME ARM
; 0 ;NO COMMAND BITS
; 0 ;DONT WOBBLE
; 0 ;NO NEXT SEGMENT
; 0 ;NO FUNCTION TIME
; 0 ;NO TRANSFORM
; CODE ;PTR TO CODE TO BE SCHEDULED THIS SEG
; ;NO POLYNOMIAL TO FOLLOW
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,CENTER
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
CENTER: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;UPDATE CURRENT POSITION
MOV R0,-(SP) ;SAVE R0
MOV #CWRE,R0 ;POINT TO COEFF. LIST FOR WHERE
MOV #CDVL,R1 ;POINT TEMPORARY DEVICE LIST
JSR PC,WHERE
TST R0
BEQ 3$ ;BRANCH IF NO WHERE ERROR
3$: MOV (SP)+,R0
MOV (SP),R1 ;RESTORE R1
;ATTACH REQUIRED SERVOS
MOV R0,CNTBLK ;SAVE POINTER TO COEF. DATA LIST
MOV #CNTBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC .+6
JMP CNTDNE ;BRANCH IF ATTACH ERROR OCCURRED
CMP #7,R0 ;CHECK THAT EXACTLY 7 JOINTS ATTACHED
BEQ 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR IF NOT THE RIGHT NUMBER OF JTS
JMP CNTDNE
;COMPUTE THE CHANGE IN JOINT ANGLES FOR A MOTION ALONG THE SWEEP DIRECTION
1$: MOV #YELARM,R2 ;ASSUME YELLOW ARM ATTACHED
BIT #YHAND,@CNTBLK ;CHECK WHICH HAND THIS IS
BNE .+6 ;SKIP IF IT IS THE YELLOW HAND
MOV #BLUARM,R2 ;ELSE INDICATE BLUE ARM
MOV #THPPTR,R1 ;GET POINTERS TO ALL JOINT ANGLES
MOV #CNTTRN,R0 ;STORE CURRENT ARM TRANSFORM HERE
JSR PC,UPDATE ;COMPUTE ARM TRANSFORM
LDF CNTTRN+12.,AC0 ;MODIFY THE POSITION BY THE ORIENTATION UNIT VECTOR
ADDF CNTTRN+36.,AC0
STF AC0,CNTTRN+36.
LDF CNTTRN+16.,AC0
ADDF CNTTRN+40.,AC0
STF AC0,CNTTRN+40.
LDF CNTTRN+20.,AC0
ADDF CNTTRN+44.,AC0
STF AC0,CNTTRN+44.
MOV #CNTTRN,R0 ;POINT TO NEW TRANSFORM
MOV #THPPTR,R1 ;PUT NEW JOINT ANGLES IN HERE
JSR PC,SOLVE ;COMPUTE NEW JOINT ANGLES FOR CHANGE
TST R0 ;CHECK IF SOLUTION EXISTS
BEQ 2$ ;BRANCH IF IT DOES
MOV #NOSOLU,R0 ;ELSE SIGNAL ERROR
BR CNTDNE
;SET THE CHANGE IN JOINT ANGLE IN ALL ARM JOINTS
2$: MOV #BSRVOS,R0 ;GET POINTER TO BLUE ARM SERVOS
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE .+6
MOV #YSRVOS,R0 ;REPLACE WITH POINTER TO YELLOW ARM IF NECESSARY
MOV #7,R3 ;WANT TO SET DELTHD FOR ALL SIX JOINTS AND HAND
LDF JTRATE,AC1 ;LOAD THE % CHANGE IN JT ANGLE PER MILLISECOND
BR SETCTR
CNTLP: LDF THP(R1),AC0 ;LOAD FINAL PLANNED JOINT ANGLE
SUBF TH(R1),AC0 ;GET TOTAL DIFFERENTIAL CHANGE
MOV TH(R1),THP(R1) ;RESTORE PREDICTED POSITION
MULF AC1,AC0 ;GET CHANGE PER MILLISECOND
MOV TH+2(R1),THP+2(R1)
STF AC0,DELTHD(R1) ;SET DIFFERENTIAL VELOCITY
SETCTR: MOV (R0)+,R1 ;GET POINTER TO ARM SERVO DATA BLOCK
MOV TH(R1),DELTH(R1) ;INITIAL JOINT ANGLE SET TO PRESENT ANGLE
MOV TH+2(R1),DELTH+2(R1)
LDF NCI(R1),AC0 ;SET GRAVITY LOADING AND INERTIA EQUAL TO
STF AC0,CI(R1) ; INITIAL VALUES
LDF NCII(R1),AC0
STF AC0,CII(R1)
SOB R3,CNTLP ;DO ALL JOINTS
MOV #32.,TTIME(R1) ;ONLY DELAY STARTING HAND BY 32 MILL. SECONDS
MOV RATE1,DELTHD(R1) ;SET INITIAL RATE OF HAND CLOSING
MOV RATE1+2,DELTHD+2(R1)
;INITIATE ON MONITOR AND START SERVOS RUNNING
MOV #CTRBLU,R3 ;GET PTR TO BLUE ARM TOUCH SENSOR DATA BLOCK
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE .+6
MOV #CTRYEL,R3 ;REPLACE WITH PTR TO YELLOW TOUCH SENSORS IF NEC
MOV (SP),CTRDVS(R3) ;SAVE PTR TO DEVICE BLOCK
CLR NUMTCH(R3) ;BOTH TOUCH SENSORS OFF TO BEGIN WITH
.IFZ SNGSTP
EVMAK ;CREATE AN EVENT FOR THE RIGHT TOUCH SENSOR
MOV (SP),RTTEVT(R3) ;SAVE EVENT NUMBER
FORK RTTPDB(R3),#CNTRRT,#USRDM;FIRE UP RIGHT T.S. ON MONITOR, LEVEL 5
.ENDC
MOV (SP),R0 ;SCHEDULE SIGNALING OF EVENT WHEN TOUCH SENSOR ON
MOV (R3),R1 ;DATA FOR RIGHT TOUCH SENSOR
JSR PC,TOUCH ;SCHEDULE EVENT SIGNALLING
.IFZ SNGSTP
EVMAK ;REPEAT FOR LEFT TOUCH SENSOR
MOV (SP),LFTEVT(R3) ;SAVE LEFT EVENT NUMBER
FORK LFTPDB(R3),#CNTRLF,#USRDM;FIRE UP LEFT T.S. ON MONITOR, LEVEL 5
.ENDC
MOV (SP),R0 ;SCHEDULE SIGNALING OF EVENT WHEN TOUCH SENSOR ON
MOV LFTCHS(R3),R1
JSR PC,TOUCH
MOV CNTBLK,R0 ;PTR TO COEF. LIST
MOV 4(SP),R1 ;PTR TO DEVICE BLOCK
JSR PC,RUNSRV ;GO RUN THE SERVOS
MOV R0,R2 ;SAVE RUN TIME ERROR CODES
.IFZ SNGSTP
MOV (SP),R0 ;CLEAR ALL HANGING EVENTS
MOV LFTCHS(R3),R1 ;TAKE EVENT OFF TOUCH LIST
JSR PC,CLRTCH
EVKIL ;DELETE EVENT NUMBER AND KILL ANY ATTACHED PROCESS
MOV (SP),R0 ;REPEAT FOR RIGHT TOUCH SENSOR
MOV (R3),R1
JSR PC,CLRTCH
EVKIL
.ENDC
MOV R2,R0 ;RESTORE RUN TIME ERROR CODES
CMP #2,NUMTCH(R3) ;CHECK IF CENTER FUNCTION EXEC. PROPERLY
BNE CNTDNE ;EXIT IF NOT BOTH TOUCH SENSORS ACTIVIATED
MOV (SP),R1 ;ELSE CLEAR "TMEOUT" BIT FOR HAND, THIS IS
MOV 20(R1),R1 ; THE NORMAL END CONDITION
BIC #TMEOUT,(R1)
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
CNTDNE: MOV (SP)+,R1 ;RESTORE POINTER TO DEVICE BLOCK
JSR PC,RELSRV ;RELEASE ATTACHED SERVOS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "CENTER"
DATA
CNTTRN: .BLKW 24. ;STORAGE AREA FOR ARM TRANSFORM
;INFORMATION FOR SERVO DATA BLOCKS
CNTBLK: 0 ;POINTER TO COEF. DATA LIST
15000. ;ALLOW 15 SECONDS FOR FUNCTION
16000. ;DELAY STARTING BY ALL JOINTS BUT HAND UNTIL TOUCH
0 ;NO POLYNOMIALS
CWRE: 774 ;WHERE ARE ALL 7 JOINTS OF BLUE ARM
0
CDVL: .BLKW 20 ;STORAGE FOR WHERE
CODE
;END OF "CENTER"
;CNTRRT & CNTRLF - ON MONITOR FOR CENTER FUNCTION
;THIS ROUTINE IS SCHEDULED FOR EXECUTION WHENEVER EITHER OF THE TOUCH SENSORS
;ON AN ARM PERFORMING A "CENTER" FUNCTION INITIALLY SWITCHES TO THE "ON"
;STATE. IF ONLY ONE TOUCH SENSOR IS ON, THE SPEED AT WHICH THE HAND IS CLOSING
;IS REDUCED AND THE JOINT SERVOS OF THE ARM START RUNNING. THE GROSS MOTION
;OF THE ARM IS INTENDED TO NEGATE THE SPEED AT WHICH THE HAND IS CLOSING TO
;PRODUCE A NET VELOCITY OF ZERO FOR THE TOUCH SENSOR IN CONTACT WITH AN OBJECT.
;IF BOTH TOUCH SENSOR ARE NOW ON, ARM JOINT MOTION IS STOPPED AND THE HAND IS
;PERMITTED TO CLOSE EVENLY ABOUT THE OBJECT. THIS ROUTINE HAS TWO ENTRY POINTS
;WHICH ARE USED FOR DIFFERENTIATING BETWEEN TOUCH SENSORS. IT IS ASSUMED THAT
;WHEN EITHER OF THESE ROUTINES IS CALLED, REGISTER R5 CONTAINS A POINTER TO
;EITHER THE "CTRYEL" OR THE "CTRBLU" DATA BLOCKS.
;ENTRY POINT FOR TOUCH SENSOR ON RIGHT SIDE OF HAND
CNTRRT: MOV #100000,R3 ;INDICATE RT. T.S. ENTRY TAKEN
MOV RTTEVT(R5),-(SP) ;PUT EVENT NUMBER ON STACK
BR MONSTR
;ENTRY POINT FOR TOUCH SENSOR ON LEFT SIDE OF HAND
CNTRLF: CLR R3 ;INDICATE LEFT T.S. ENTRY TAKEN
MOV LFTEVT(R5),-(SP) ;PUT EVENT NUMBER OF STACK
;COMMON CODE FOR BOTH ENTRY POINTS
MONSTR:
.IFZ SNGSTP
EVWAIT ;WAIT TILL THIS EVENT IS TRIGGERED
BCS MONDNE ;JUST DIE IF EVENT WAS KILLED INSTEAD OF SIGNALED
.ENDC
MOV CTRDVS(R5),R1 ;GET PTR TO DEVICE BLOCK
ADD #4,R1 ;GET PTR TO FIRST SERVO DATA BLOCK ATTACHED
MOV #6,R2 ;FIX UP 6 ARM JOINTS
TST NUMTCH(R5) ;CHECK IF THIS IS THE FIRST TOUCH SENSOR ON
BNE 2$ ;IF SECOND TOUCH, GO STOP JOINT MOTION
;ONE TOUCH SENSOR ON, START JOINT MOTION
1$: MOV (R1)+,R0 ;GET PTR TO JOINT SERVO
XOR R3,DELTHD(R0) ;INSURE JOINT GOING IN PROPER DIRECTION
MOV PTIME(R0),TTIME(R0) ;START JOINT MOVING
SOB R2,1$ ;REPEAT FOR ALL ATTACHED JOINTS
MOV (R1)+,R0 ;GET PTR TO HAND SERVO
MOV RATE2,DELTHD(R0) ;SLOW DOWN RATE OF HAND CLOSING
MOV RATE2+2,DELTHD+2(R0)
BR 3$ ;INDICATE 1 TOUCH SENSOR ON
;BOTH TOUCH SENSORS ON, STOP JOINT MOTION
2$: MOV (R1)+,R0 ;GET PTR TO JOINT SERVO
BIC #RUN,(R0) ;TELL JOINT TO STOP MOTION
SOB R2,2$ ;REPEAT FOR ALL ARM JOINTS, NO HAND
MOV (R1)+,R0 ;ALLOW HAND TO CLOSE FOR A FEW MORE MIL-SEC.
MOV #CTRSTP,COUNT(R0)
3$: INC NUMTCH(R5) ;INCREMENT THE NUMBER OF TOUCH SENSORS ON
;EXIT CLEANLY
MONDNE:
.IFZ SNGSTP
DISMISS ;ALL DONE
.IFF
JMP RUG
.ENDC
;END OF "MONCTR"
;DATA FOR CENTER OPERATIONS
DATA
;RELATIVE POINTERS FOR "CTRYEL" AND "CTRBLU" DATA BLOCKS
RTTCHS ==0 ;SENSOR DATA TO SEND TO "TOUCH" SUBROUTINE
LFTCHS ==2
RTTEVT ==4 ;EVENT NUMBER FOR WAITING FOR RIGHT TOUCH SENSOR ACTIVE
LFTEVT ==6 ;SAME FOR LEFT
RTTPDB ==10 ;PTR TO PROCESS DESCRIPTOR BLOCK FOR CODE TO RUN ON RT. TCH.
LFTPDB ==12 ;SAME FOR LEFT
NUMTCH ==14 ;NUMBER OF TOUCH SENSORS ACTIVE: 0,1,2
CTRDVS ==16 ;PTR. TO DEVICE BLOCK FOR ARM PERFORMING "CENTER" FUNCTION
;DATA FOR OPERATING WITH YELLOW ARM
CTRYEL: 000402 ;TRIGGER IF YELLOW HAND RT SENSOR "ON"
000403 ; " " " " LF " "
0 ;EVENT NUMBER FOR RIGHT TOUCH SENSOR
0 ; " " " LEFT " "
YRTPDB ;PTR TO PDB FOR CODE TO RUN WHEN RT SENSOR ACTIVATED
YLFPDB ; " " " " " " " " LF " "
0 ;BOTH TOUCH SENSORS INITIALLY OFF
0 ;POINTER TO DEVICE BLOCK
;DATA FOR OPERATING WITH BLUE ARM
CTRBLU: 000400 ;TRIGGER IF BLUE HAND RT SENSOR "ON"
000401 ; " " " " LF " "
0 ;EVENT NUMBER FOR RIGHT TOUCH SENSOR
0 ; " " " LEFT " "
BRTPDB ;PTR TO PDB FOR CODE TO RUN WHEN RT SENSOR ACTIVATED
BLFPDB ; " " " " " " " " LF " "
0 ;BOTH TOUCH SENSORS INITIALLY OFF
0 ;POINTER TO DEVICE BLOCK
;RATES AT WHICH JOINTS AND HAND MOVE
RATE1: .FLT2 -.005 ;INITAL RATE OF HAND CLOSING
RATE2: .FLT2 -.0005 ;RATE OF HAND CLOSING AFTER TOUCH
JTRATE: .FLT2 .00025 ;DIFFERENTIAL CHANGE IN JT ANGLE/MSEC.
;ON MONITOR PROCESSOR DESCRIPTOR BLOCKS
YRTPDB: PDBLK 5,20,,CTRYEL
YLFPDB: PDBLK 5,20,,CTRYEL
BRTPDB: PDBLK 5,20,,CTRBLU
BLFPDB: PDBLK 5,20,,CTRBLU
CODE
;END OF "CENTER" DATA
;WHERE - UPDATES THE CURRENT JT ANGLE AND DYNAMIC COEF FOR SERVOS
;THE SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE ATTACHED AND THEIR
;JOINT ANGLE, GRAVITY LOADING, AND JOINT INERTIA VALUES ARE UPDATED IN THEIR
;RESPECTIVE SERVO DATA BLOCKS. ONLY THE FIRST TWO WORDS OF THE COEFFICIENT
;LIST ARE USED BY THIS ROUTINE.
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS
; XXXXXX
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK
; JSR PC,WHERE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0 IS GARBAGED
WHERE: MOV R0,WHRBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV #WHRBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS 2$ ;EXIT IF ATTACH ERROR RETURNED
TST R0 ;BRANCH IF AT LEAST ONE SERVO ATTACHED
BGT 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR 2$ ;EXIT
;READ JOINT ANGLES AND UPDATE CURRENT JOINT INERTIA AND GRAVITY LOADING
1$: JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL ATTACHED SERVOS
BCS 2$ ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO PREPARATION ERROR
;EXIT CLEANLY, RELEASE ALL ATTACHED SERVOS
2$: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
WHRBLK: 0 ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
CODE
;END OF "WHERE"
;MOVE - MOVES THE ARM JOINTS ALONG A PRECOMPUTED TRAJECTORY
;THE JOINT SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE DRIVEN
;ACCORDING TO PREDETERMINED POLYNOMIALS REPRESENTING THE JOINT ANGLES OR
;EXTENTIONS. ANY COMBINATION OF ARM JOINTS AND HAND JOINTS FROM THE
;"YELLOW" AND "BLUE" ARMS CAN BE SPECIFIED AS A SINGLE LOGICAL DEVICE.
;ANY PLANNED TRAJECTORY THAT IS FOUND TO BE INVALID DUE TO A CHANGE IN
;THE START OR END POINTS OF ANY OF ITS SEGMENTS IS CORRECTED BY THIS
;ROUTINE. TRAJECTORY MODIFICATION IS ACCOMPLISHED BY ADDING FIFTH ORDER
;POLYNOMIALS TO THE PLANNED POLYNOMIALS. CORRECTIONS ARE ALWAYS APPLIED
;BETWEEN VIA POINTS. IF TWO VIA POINTS HAVE ONE OR MORE FREE POINTS
;BETWEEN THEM, ALL OF THE SEGMENTS BETWEEN THE VIA POINTS ARE TREATED AS
;ONE SINGLE SEGMENT AND THE FIFTH ORDER CORRECTION POLYNOMIAL IS APPLIED
;ACROSS THIS SINGLE LONG SEGMENT.
;
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON
;PAGE 10 OF THIS PROGRAM. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE
;BLOCK. THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS
;SUCCESSFULLY ATTACHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE
;THIRD WORD OF THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE
;LOGICAL NUMBER OF ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN
;THAT SERVOS STATUS ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.ifz short
.IFZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;SET UP DATA ARRAY TO BE TRANSFERRED
MOV @FPSAV,GPTR ;GET POINTERS FOR DATA GATHERING
MOV @ISAV,GIPTR
;ATTACH REQUIRED SERVOS
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 1$ ;BRANCH IF NO ATTACH ERRORS
JMP MVEDNE ;ERROR RETURNED, SIGNAL ATTACH ERROR AND EXIT
1$: TST R0 ;BRANCH IF AT LEAST ONE JOINT SELECTED
BGT 2$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
JMP MVEDNE ;EXIT CLEANLY
;SET UP JOINT WOBBLING IF NECESSARY
2$: BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 4$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 3$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
3$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 4$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;GET POINTER TO FIRST SEGMENT DATA AND DETERMINE IF THE STARTING POINT IS
;WITHIN JOINT ANGLE ERROR TOLERANCE. THE CHANGES IN THE JOINT ANGLES ARE
;COMPUTED AND SAVED IN "LJTDIF".
;*K SKIP IF VISE...
;4$: BITB #VISE,MECHSM(R4);TEST IF VISE *K
; BNE XXX ;BRANCH IF POLY. MODIF. NOT NEEDED *K
4$: ADD #DATST,MVEBLK ;GET ADDR OF 1ST SEGMENT DATA
MOV MVEBLK,R5
ADD #A0COEF,R5 ;PTR TO 1ST POLYNOMIAL COEF.
CLRB ISLD ;ASSUME ALL JOINTS WITHIN TOLERANCE
MOV #MCHDAT,R0 ;STORE INFORMATION ABOUT MECHANISMS IN HERE
MOV #LJTDIF,R2 ;STORE DIFFERENCE IN JOINT ANGLES IN HERE
MOVB (R1),R3 ;GET THE NUMBER OF JOINT SERVOS ATTACHED
CMP (R1)+,(R1)+ ;GET ADDR OF 1ST SERVO DATA BLOCK
MOV (R1),R4
BR SAVMCH ;SAVE NEW MECHANISM INFORMATION
MVCMPT: CMPB MECHSM(R4),(R0) ;CHECK IF FIRST SERVO OF NEW MECHANISM
BEQ OLDMCH ;BRANCH IF SAME MECHANISM
ADD #10,R0 ;ELSE SAVE NEW MECHANISM INFORMATION
SAVMCH: MOVB MECHSM(R4),(R0) ;SAVE MECHANISM BIT
CLRB 1(R0) ;INIT. COUNT OF # OF SERVOS FOR THIS MECH
MOV R1,2(R0) ;PTR TO FIRST SERVO FOR MECH
MOV R5,4(R0) ;REL. PTR TO FIRST POLYNOMIAL COEF.
SUB MVEBLK,4(R0)
MOV R2,6(R0) ;REL. PTR TO CHANGE IN JOINT ANGLE
SUB #LJTDIF,6(R0)
OLDMCH: INCB 1(R0) ;ONE MORE SERVO IN THIS MECH
LDF THP(R4),AC0 ;COMPUTE THE ERROR IN POSITION, TH-A0
SUBF (R5),AC0 ;SUBTRACT THE PLANNED JOINT POSITION
STF AC0,AC1
ABSF AC0
CMPF ERRTOL(R4),AC0 ;COMPARE TO ERROR TOLERANCE
CFCC
BGE 1$ ;SKIP IF WITHIN TOLERANCE
BISB MECHSM(R4),ISLD ;ELSE INDICATE MECHANISM OUT OF PLACE
BR .+4
1$: CLRF AC1 ;WITHIN TOLERANCE, SO ZERO DIFFERENCE
STF AC1,(R2)+ ;SAVE CHANGE IN JOINT ANGLE
ADD #A5COEF-A0COEF+4,R5 ;POINT TO NEXT JOINT STARTING ANGLE
TST (R1)+ ;GET ADDR OF NEXT SERVO DATA BLOCK
MOV (R1),R4
SOB R3,MVCMPT ;DO FOR ALL ATTACHED SERVOS
CLR 10(R0) ;INDICATE END OF MECHANISM DATA LIST
["MOVE"]
;IF THIS SEGMENT IS FOR A DEPARTURE POINT, SET A BIT MASK TO INVALIDATE ALL
;MECHANISM TRANSFORMS FOR MECHANISMS WHOSE PLANNED STARTING POSITION WAS NOT
;CORRECT.
MOV MVEBLK,R5 ;GET POINTER TO FIRST SEGMENT DATA
CLRB INVDEP ;ASSUME NO DEPARTURE POINT PLANNED
BIT #DEPTPT,OPBITS-DATST(R5) ;CHECK IF WE HAVE A DEPARTURE POINT
BEQ UPDTRN ;BRANCH IF NONE
MOVB ISLD,INVDEP ;ELSE INVALIDATE TRANSFORMS FOR JOINTS WITH
; ERRORS IN THEIR STARTING POINTS
;CHECK IF CURRENT VIA POINT HAS A TRANSFORM. IF IT DOESNT, IT MUST BE A FREE
;POINT SO SKIP DOWN TO THE NEXT SEGMENT. ONCE WE GET A TRANSFORM, DETERMINE
;IF IT IS VALID.
UPDTRN: CLRB ISUD ;ASSUME FINAL TRANSFORMS ALL UNCHANGED
MOV R5,STRSEG ;SAVE POINTER TO CURRENT SEGMENT
MOV SEGTME(R5),MVETME ;SAVE CURRENT SEGMENT ELAPSED TIME
TSTTRN: MOV R5,FINSEG ;SAVE PTR TO FIRST NON-FREE POINT SEGMENT
TST SEGTRN(R5) ;CHECK IF FREE POINT
BNE 1$ ;BRANCH IF NOT A FREE POINT
ADD (R5),R5 ;SKIP TO NEXT SEGMENT DATA
TST (R5) ;CHECK IF JUST A CONSTANT FOR A END POINT
BEQ NOTRNS ;BRANCH IF SO
ADD SEGTME(R5),MVETME ;ACCUMULATE TOTAL ELAPSED TIME BETWEEN VIA PTS
BR TSTTRN ;REPEAT
;LOOP FOR CORRECTING DISCREPENCIES IN FINAL POSITIONS FOR EACH MECHANISM
1$: MOV SEGTRN(R5),-(SP) ;GET POINTER TO TRANSFORM/VALIDITY LIST
MOV #MCHDAT,-(SP) ;GET PTR TO MECHANISM DATA
MVECKT: MOV 2(SP),R2 ;RESTORE TRANS/VALIDITY LIST PTR
MOV (SP),R3 ;RESTORE MECHANISM DATA PTR
MOV 10(SP),R4 ;GETARG NEEDS ORIGINAL R4
MOV (R2),R0 ;SEND TRANS INFORMATION TO GETARG
JSR PC,@LGETARG
TSTB 1(R0) ;See if we have a trans or a frame
BEQ 2$ ;Branch if trans
MOV 2(R0),R0 ;GET LOC(frame header)
BITB (R3),INVDEP ;CHECK IF THIS IS A INVALID DEPARTURE POINT
BEQ 1$ ;BRANCH IF IT ISN'T
MOV R0,-(SP)
JSR PC,@LINVLDT ;ELSE, MARK THIS MECHANISM DEPARTURE POINT INVALID
MOV (SP)+,R0
1$: CALL @LGETVAL,R0 ;R0← TRANS POINTER
BR 3$
2$: MOV 2(R0),R0 ;R0 ← Trans pointer
3$:
; CMP 2(R2),R1 ;CHECK IF CORRECT VALIDITY NUMBER
; BEQ MVISVD ;BRANCH IF IT'S VALID
;TRANSFORM FOUND TO BE INVALID, COMPUTE NEW FINAL JOINT ANGLES AND SAVE
BISB (R3),ISUD ;INDICATE MECH. TRANSFORM INVALID
MOV R1,-(SP) ;SAVE NEW VALIDITY NUMBER
BITB (R3),ISLD ;CHECK IF MOST RECENT LOWER POSITIONS NOT SAVED
BNE GOTLOW ;BRANCH IF WE ALREADY HAVE THEM
MOVB 1(R3),R4 ;ELSE NEED MOST RECENT ANGLES FOR ARM SOLUTION PROG
MOV 2(R3),R1 ;GET PTR TO ATTACHED MECHANISM SERVO BLOCKS
MOV STRSEG,R5 ;PTR TO FIRST MECHANISM POLYNOMIAL
ADD 4(R3),R5
4$: MOV (R1)+,R2 ;GET SERVO DATA BLOCK PTR
LDF (R5),AC0 ;UPDATE ALL INITIAL ANGLES FOR THIS MECHANISM
STF AC0,THP(R2)
ADD #A5COEF-A0COEF+4,R5 ;POINT TO THE NEXT START ANGLE
SOB R4,4$ ;REPEAT FOR ALL MECHANISM SERVOS
GOTLOW: MOV #YTHP,R1 ;GET POINTER TABLE TO INITIAL JOINT ANGLES
MOVB (R3),R2 ;GET MECHANISM NUMBER
JSR PC,SOLVE ;COMPUTE THE NEW FINAL JOINT ANGLES
TST R0 ;CHECK IF NO SOLUTION FOUND
BEQ 1$ ;BRANCH IF SOLUTION OK
ALERR NOSOLM ;TELL OPERATOR, NO ARM SOLUTION EXISTS
TST R0 ;CHECK IF HE WANTS TO TRY THIS MOTION ANYWAY
BEQ 1$ ;BRANCH IF SO
MOV #NOSOLU,R0 ;ELSE SIGNAL NO ARM SOLUTION FOUND
ADD #6,SP ;CLEAR STACK
JMP MVEDNE ;EXIT CLEANLY
;COMPUTE END POINT DIFFERENCE FOR MECHANISM WITH INVALID TRANFORMS
1$: MOVB 1(R3),R4 ;GET NUMBER OF DIFFERENCES TO COMPUTE
MOV 2(R3),R1 ;GET PTR TO ATTACHED MECHANISM SERVO BLOCKS
MOV FINSEG,R5 ;PTR TO MECHANISM FIRST SERVO, FINAL POLYNOMIAL
ADD 4(R3),R5
MOV 6(R3),R3 ;GET ADDR TO START SAVING CHANGES
ADD #UJTDIF,R3
2$: MOV (R1)+,R0 ;GET SERVO DATA BLOCK ADDRESS
LDF THP(R0),AC0 ;GET THE NEW JOINT ANGLE
MOV #6,R2 ;OLD JT ANGLE ← SUM OF POLYNOMIAL COEF.
SUBF (R5)+,AC0 ;SUBTRACT THE OLD JOINT ANGLE FROM THE NEW
SOB R2,.-2
STF AC0,TJTDIF-UJTDIF(R3) ;SAVE DIFFERENCES
STF AC0,(R3)+
SOB R4,2$ ;REPEAT FOR ALL MECHANISM SERVOS
;SAVE NEW TRANSFORM VALIDITY NUMBER
MOV 4(SP),R2 ;RESTORE TRANS/VALIDITY LIST PTR
MOV (SP)+,2(R2) ;SAVE NEW VALIDITY NUMBER IN COEF. LIST
;UPDATE POINTERS FOR NEXT MECHANISM AND CHECK FOR END OF LOOP
MVISVD: ADD #4,2(SP) ;POINT TO NEXT TRANS BLOCK
ADD #10,(SP) ;POINT TO NEXT MECHANISM DATA
TSTB @(SP) ;TEST FOR NO MORE MECHANISMS
BNE MVECKT ;REPEAT TILL ALL DONE
ADD #4,SP ;CLEAR THE STACK
NOTRNS: CLRB INVDEP ;ONLY 1 DEPARTURE POINT PER MOTION
["MOVE"]
;COMPUTE THE TOTAL CHANGE IN JOINT ANGLES BETWEEN VIA POINTS = UPPER
; DIFFERENCE - LOWER DIFFERENCE.
TST ISLD ;CHECK IF ANY DIFFERENCE TO CORRECT
BEQ NONWLD ;BRANCH IF NO CORRECTIONS TO DO
MOV STRSEG,R5 ;SWITCH TO SEGMENT DATA FOR START POINT
CLR MVESTM ;FIRST SEGMENT STARTS AT TIME=0
ADDDIF: MOV #MCHDAT,R4 ;GET PTR TO MECHANISM DATA
ADDLD: BITB (R4),ISLD ;CHECK IF THIS MECH. HAS A LOWER DIFFERENCE
BEQ DIFNXT ;BRANCH IF NONE
MOVB 1(R4),R1 ;NUMBER OF JOINTS TO MODIFY
MOV 4(R4),R0 ;POINT TO START OF POLYNOMIAL COEF
ADD R5,R0
MOV 6(R4),R2 ;GET PTR TO TOTAL JT CHANGE ARRAY
ADD #TJTDIF,R2
1$: LDF LJTDIF-TJTDIF(R2),AC0 ;GET LOWER DIFFERENCE
TST MVESTM ;CHECK IF FIRST SEGMENT
BNE 2$ ;BRANCH IF TOTAL DIFFERENCE ALREADY COMPUTED
CLRF AC1 ;ASSUME NO UPPER DIFFERENCE
BITB (R4),ISUD
BEQ .+6
LDF UJTDIF-TJTDIF(R2),AC1
SUBF AC0,AC1
STF AC1,(R2)
2$: ADDF (R0),AC0 ;CORRECT INITIAL POSITION
STF AC0,(R0)
ADD #A5COEF-A0COEF+4,R0 ;POINT TO NEXT POLYNOMIAL
ADD #4,R2 ;POINT TO NEXT ENTRY IN DIFFERENCE TABLE
SOB R1,1$
DIFNXT: ADD #10,R4
TSTB (R4)
BNE ADDLD
;ADD IN 5TH ORDER CORRECTION EQUATION
MOV #MVE5TH,R0 ;GENERATE 5TH ORDER CORRECTION COEF. IN HERE
MOV MVETME,R1 ;TOTAL TIME BETWEEN VIA POINTS
MOV SEGTME(R5),R2 ;ELAPSE TIME FOR THIS SEGMENT
MOV MVESTM,R3 ;START TIME FOR THIS SEGMENT
JSR PC,MAK5TH
MOV R5,-(SP) ;SAVE SEGMENT POINTER
MOV #MCHDAT,-(SP) ;CORRECT ALL POLYNOMIALS THAT ARE INCORRECT
ADD5LP: BITB @(SP),ISLD ;CHECK IF THIS MECHANISM NEEDS MODIFICATION
BNE 1$
BITB @(SP),ISUD
BEQ NO5TH ;BRANCH IF NO 5TH ORDER CORRECTION NECESSARY
1$: MOV (SP),R4 ;GET MECHANISM DATA
MOVB 1(R4),R5 ;NUMBER OF POLYNOMIALS TO MODIFY
MOV 2(R4),R0 ;POINTER TO SERVO DATA BLOCKS
MOV 4(R4),R3 ;GET POINTER TO POLYNOMIAL COEF.
ADD 2(SP),R3
MOV 6(R4),R2 ;TOTAL JOINT DISPLACEMENTS STORED HERE
ADD #TJTDIF,R2
COR5TH: MOV #MVE5TH,R1 ;POINT TO CORRECTION POLYNOMIAL
LDF (R2)+,AC0 ;LOAD TOTAL DISPLACEMENT FOR THIS JOINT
CLRF AC2 ;NEW FINAL POSITION IN HERE
MOV #6,R4 ;ADD 6 TERMS OF CORRECTION POLY TO ORIGINAL POLY
1$: LDF (R1)+,AC1 ;SCALE 5TH ORDER FOR TOTAL DISPLACEMENT
MULF AC0,AC1
ADDF (R3),AC1 ;ADD TO ORIGINAL POLYNOMIAL
STF AC1,(R3)+
ADDF AC1,AC2 ;GET NEW FINAL ANGLE
SOB R4,1$
MOV (R0)+,R4 ;SAVE NEW ANGLE
STF AC2,THP(R4)
SOB R5,COR5TH ;DO ALL POLYNOMIALS FOR THIS MECHANISM
; MOV @(SP),R2 ;GET MECHANISM BITS
; BIT #YELARM+BLUARM,R2 ;CHECK IF WE NEED TO CHANGE DYN. COEF
; BEQ NOT5TH ;BRANCH IF NO CHANGE NECESSARY
; MOV #YCI,R1 ;ASSUME YELLOW ARM COEF. TO BE CHANGED
; MOV #YTHP,R0 ;POINT TO THE JOINT ANGLES
; BIT #YELARM,R2 ;CHECK IF YELLOW ARM OR BLUE ARM
; BNE .+12
; MOV #BCI,R1 ;REPLACE WITH BLUE ARM POINTERS IF NEEDED
; MOV #BTHP,R0
; JSR PC,DTERMS ;COMPUTE NEW DYNAMIC COEF. FOR END POINT
; MOV (SP),R4 ;SAVE NEW COEFFICIENTS IN POLYNOMIAL DATA LIST
; MOVB 1(R4),R5 ;NUMBER OF DYNAMIC COEFFICENTS TO MODIFY
; MOV 2(R4),R0 ;POINTER TO SERVO DATA BLOCKS
; MOV 2(SP),R3 ;GET POINTER TO DYNAMIC COEF.
; SUB MVEBLK,R3
; MOV (R0)+,R1 ;GET POINTER TO SERVO DATA BLOCKS
; ADD FCI(R1),R3
;FILCII:LDF CI(R1),AC0 ;GET NEW GRAVITY LOADING AND SAVE
; STF AC0,(R3)+
; LDF CII(R1),AC0 ;GET NEW INERTIA AND SAVE
; STF AC0,(R3)+
; MOV (R0)+,R1 ;REPEAT FOR NEW SERVO
; SOB R5,FILCII
NO5TH: ADD #10,(SP) ;POINT THE NEXT MECHANISM
TSTB @(SP) ;REPEAT IF ANY MORE
BNE ADD5LP
TST (SP)+ ;CLEAR STACK
MOV (SP)+,R5 ;GET CURRENT SEGMENT POINTER
CMP FINSEG,R5 ;CHECK IF ALL FREE POINTS DONE
BEQ MVNXSG ;GO ON TO NEXT SEGMENT IF DONE
ADD SEGTME(R5),MVESTM ;UPDATE START TIME
ADD (R5),R5 ;POINT TO THE NEXT SEGMENT DATA
JMP ADDDIF ;GO REPEAT FOR NEXT SEGMENT
["MOVE"]
;SET UP FOR THE NEXT SEGMENT IF THERE IS ONE
MVNXSG: SWAB ISLD ;IF THE LAST SEG. HAD A CHANGE IN END POSITION
; THE NEXT ONE WILL HAVE A CHANGE IN START POS.
BEQ NONWLD ;BRANCH IF NO LOWER DIFF. FOR NEXT SEG.
MOVB @(SP),R0 ;ELSE COPY THE PREVIOUS UPPER DIFF INTO LOWER DIFF
MOV #UJTDIF,R1 ;FROM HERE
MOV #LJTDIF,R2 ;TO HERE
1$: MOV (R1)+,(R2)+ ;TRANSFER DIFFERENCES
MOV (R1)+,(R2)+
SOB R0,1$
NONWLD: MOV FINSEG,R5 ;GET POINTER TO NEXT SEGMENT
ADD (R5),R5
TST (R5) ;CHECK IF LAST SEGMENT
BEQ 1$ ;IF SO, GO INITIATE SERVOS
JMP UPDTRN ;REPEAT PROCESS FOR NEXT SEGMENT
;INITIATE THE SERVOS FOR RUNNING
1$: MOV (SP),R1 ;RESTORE "THP" VARIABLES IN SERVO DATA BLOCKS
MOVB (R1),R2 ;GET NUMBER OF SERVOS TO CORRECT
ADD #4,R1 ;POINT TO FIRST SERVO
2$: MOV (R1)+,R0 ;GET SERVO DATA BLOCK POINTER
LDF TH(R0),AC0 ;RESTORE THP
STF AC0,THP(R0)
SOB R2,2$
MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF LIST
SUB #DATST,R0
JSR PC,RUNSRV
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "MOVE"
DATA
ISLD: .BYTE 0 ;TRUE IF LOWER END OF SEGMENT DISPLACED
ISUD: .BYTE 0 ; " " UPPER " " " "
INVDEP: .BYTE 0 ;BITS INDICATING DEPARTURE POINTS TO BE SET INVALID
.BYTE 0
LJTDIF: .BLKW 32. ;CORRECTION TO JOINT ANGLES START POSITION
UJTDIF: .BLKW 32. ; " " " " FINAL "
TJTDIF: .BLKW 32. ;TOTAL JOINT CORRECTION
MVETME: .BLKW 2 ;ELAPSED TIME BETWEEN VIA POINTS
STRSEG: 0 ;POINTER TO SEGMENT DATA FOR START POINT
FINSEG: 0 ; " " " " " FINAL "
MVE5TH: .BLKW 12. ;STORAGE AREA FOR 5TH ORDER CORRECTION POLYNOMIAL
MVESTM: 0 ;SEGMENT START TIME
MCHDAT: .BLKW 17. ;MECHANISM DATA, CONTAINS THE FOLLOWING:
; WD.1: MECH. NUMBER, NUMBER OF SERVO IN MECH
; WD.2: PTR TO 1ST MECH. SERVO IN DEVICE BLOCK
; WD.3: REL ADDR. OF 1ST SERVO A0 TO START OF SEG.
; WD.4: REL ADDR. OF 1ST DIFFERENCE CELL IN JTDIF ARRAYS
NOSOLM: .ASCIZ /NO VALID ARM SOLUTION FOUND. REGISTER R0 CONTAINS BITS
INDICATING WHICH JOINTS WHERE OUT OF RANGE ( 1=JT1,
2=JT2, 4=JT3, 10=JT4,20=JT5, 40=JT6 ). CLEAR R0 AND
TYPE alt P TO RUN USING AN APPROXIMATE TRAJECTORY.
/
.EVEN
;INFORMATION FOR SERVO DATA BLOCKS
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
CODE
.ENDC
;MOVE - DIAGNOSTIC MOVE INSTRUCTION WITHOUT TRAJECTORY MODIFICATION
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON PAGE
;10 OF THIS PROGRAM. A PLANNED TRAJECTORY THAT IS INVALID DUE TO A CHANGE
;IN THE START OR END POINTS OF ANY OF ITS SEGMENTS IS CORRECTED BY THIS ROUTINE.
;TRAJECTORY MODIFICATION IS ACCOMPLISHED BY ADDING FIFTH ORDER POLYNOMIALS TO
;THE PLANNED POLYNOMIALS. CORRECTIONS ARE ALWAYS APPLIED BETWEEN VIA POINTS.
;IF TWO VIA POINTS HAVE ONE OR MORE FREE POINTS BETWEEN THEM, ALL OF THE SEGMENTS
;BETWEEN THE VIA POINTS ARE TREATED AS ONE SINGLE SEGMENT AND THE FIFTH ORDER
;CORRECTION POLYNOMIAL IS APPLIED ACROSS THIS SINGLE LONG SEGMENT.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. IN ADDITION, IF NO ERRORS OCCUR DURING
;PREPARATION, R1 RETURNS THE NUMBER OF SERVOS ATTACHED AND EACH WORD OF THE
;DEVICE BLOCK CONTAINS THE LOGICAL NUMBER OF ONE OF THE ATTACHED SERVOS IN
;ITS LOWER BYTE AND THAT SERVOS STATUS ERROR BITS IN THE UPPER BYTE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.IFNZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;ATTACH REQUIRED SERVOS AND DETERMINE NUMBER OF JOINTS AND ARMS TO BE RUN
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 1$ ;BRANCH IF NO ATTACH ERRORS
JMP MVEDNE ;ERROR RETURNED, EXIT CLEANLY
1$: MOV R0,MVSRCT ;SAVE NUMBER OF ATTACHED SERVOS
;SET UP JOINT WOBBLING IF NECESSARY
BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 3$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 2$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
2$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 3$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;GET POINTER TO FIRST SEGMENT DATA AND DETERMINE IF THE STARTING POINT IS
;WITHIN JOINT ANGLE ERROR TOLERANCE
3$: MOV MVEBLK,R0 ;GET POINTER TO START OF COEF. DATA LIST
ADD #DATST+A0COEF,R0 ;ADDR OF 1ST A0 COEFFICIENT
MOV MVSRCT,R3 ;GET THE NUMBER OF JOINT SERVOS ATTACHED
ADD #4,R1 ;POINT TO THE FIRST SERVO DATA BLOCK
MVCMPT: MOV (R1)+,R4 ;GET ADDRESS OF SERVO BLOCK DATA
LDF TH(R4),AC0 ;LOAD THE CURRENT JOINT ANGLE
SUBF (R0),AC0 ;COMPUTE THE ERROR IN POSITION, TH-A0
STF AC0,AC1
ABSF AC0
CMPF ERRTOL(R4),AC0 ;COMPARE TO ERROR TOLERANCE
CFCC
BGE INTOL ;SKIP IF WITHIN TOLERANCE
MOV R0,R2 ;GET THE ADDR OF THE POLYNOMIAL TO MODIFY
MOV TH(R4),(R2)+ ;SET START POINT EQUAL TO CURRENT JT ANGLE
MOV TH+2(R4),(R2)+
NEGF AC1 ;HAVE TO CHANGE BY THIS MUCH TO GET BACK
MOV #A15TH,R4 ;GET ADDRESS OF CORRECTION POLYNOMIAL
MOV #5,R5 ;CORRECT NEXT 5 COEFFICIENTS
1$: LDF (R4)+,AC0 ;GET NORMALIZED COEFFICIENT
MULF AC1,AC0 ;COMPUTE TOTAL CHANGE
ADDF (R2),AC0 ;APPLY CORRECTION
STF AC0,(R2)+ ;AND SAVE
SOB R5,1$
INTOL: ADD #A5COEF-A0COEF+4,R0 ;POINT TO NEXT JOINT STARTING ANGLE
SOB R3,MVCMPT ;DO FOR ALL ATTACHED SERVOS
;INITIATE THE SERVOS FOR RUNNING
MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR MOVE
DATA
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
MVSRCT: 0 ;NUMBER OF ATTACHED JOINTS
CODE
.ENDC
;endc for short flag
.endc
;MAK5TH - CONSTRUCTS THE 5TH ORDER POLY. USED TO MODIFY RUNTIME TRAJECTORIES
;THE 5TH ORDER CORRECTION POLYNOMIAL IS NORMALIZED TO PRODUCED A NET CHANGE OF
;1.0 FOR A CHANGE IN TIME, T. THE POLYNOMIAL IS OF THE FORM:
;
; 5 4 3 2 1
; A5*T + A4*T + A3*T + A2*T + A1*T + A0
;
;IF THE CORRECTION IS TO TAKE PLACE DURING A SINGLE SEGMENT, THE COEFFICIENTS
;ARE GIVEN AS:
; A0 = A1 = A2 = 0
; A3 = 10
; A4 = -15
; A5 = 6
;
;IF THE CORRECTION IS TO TAKE PLACE DURING THE COURSE OF SEVERAL SEGMENTS, A
;MORE GENERAL FORM FOR THE COEFFICIENTS MUST BE USED:
;
; A0 = ( 6*S**5 - 15*S**4 + 10*S**3)
; A1 = (30*S**4 - 60*S**3 + 30*S**2)*D
; A2 = (60*S**3 - 90*S**2 + 30*S )*D**2
; A3 = (60*S**2 - 60*S + 10 )*D**3
; A4 = (30*S - 15 )*D**4
; A5 = ( 6 )*D**5
; WHERE
; "D" IS THE DURATION OF THE SEGMENT UNDER CONSIDERATION.
; "S" IS THE START TIME OF THIS SEGMENT RELATIVE TO THE START
; OF THE 5TH ORDER CORRECTION POLYNOMIAL.
;
;BOTH "S" AND "D" ARE EXPRESSED AS FRACTIONS OF THE TOTAL TIME THE 5TH
;ORDER IS TO SPAN. A SAMPLE CALLING SEQUENCE TO "MAK5TH" FOLLOWS:
;
; MOV #POLY,R0 ;LEAVE NEW COEFFIENTS IN POLY ARRAY,
; ; POLY IS ARRANGED FROM A0 TO A5
; MOV TTIME,R1 ;TOTAL DURATION OF 5TH ORDER, MSEC
; MOV SEGTME,R2 ;DURATION OF THIS SEGMENT, MSEC
; MOV STRTME,R3 ;STARTING TIME OF THIS SEGMENT WITH
; ; RESPECT TO START OF 5TH ORDER, MSEC
; JSR PC,MAK5TH
;EXECUTION TIME: 390 MICRO SECONDS MAXIMUM
;REGISTERS USED:
;
; R0,R1,R2,R3 PASS ARGUMENTS AND ARE MODIFIED
; AC0,AC1,AC2,AC3 ARE GARBAGED
MAK5TH: CMP R1,R2 ;CHECK IF JUST SINGLE SEGMENT
BNE 1$ ;IF SO, GO DEVELOP MOST GENERAL FORM OF EQUATION
;SEND BACK SIMPLE 5TH ORDER FOR TOTAL CORRECTION DURING ONE SEGMENT
MOV #A05TH,R1 ;POINTER TO A1 TERM OF 5TH ORDER
MOV #12.,R2 ;TRANSFER 6 F.P. NUMBERS
MOV (R1)+,(R0)+
SOB R2,.-2
RTS PC ;RETURN
;DEVELOP 5TH ORDER IN MOST GENERAL FORM
1$: LDCIF R1,AC2 ;FLOAT TOTAL SPAN TIME
LDCIF R2,AC1 ;GET NORMALIZED SEGMENT TIME
DIVF AC2,AC1
LDCIF R3,AC0 ;GET NORMALIZED START TIME
DIVF AC2,AC0
;COMPUTE A0
LDF FP6,AC2 ;6.0
MULF AC0,AC2 ;x S
SUBF FP15,AC2 ;- 15
MULF AC0,AC2 ;x S
ADDF FP10,AC2 ;+ 10
MULF AC0,AC2 ;x S
STF AC0,AC3
MULF AC0,AC3 ;S**2
MULF AC3,AC2 ;((6*S-15)*S+10)*S**3
STF AC2,(R0)+ ;SEND BACK
;COMPUTE A1
LDF FP30,AC2 ;30
MULF AC0,AC2 ;x S
SUBF FP60,AC2 ;- 60
MULF AC0,AC2 ;x S
ADDF FP30,AC2 ;+ 30
MULF AC3,AC2 ;x S**2
MULF AC1,AC2 ;(((30*S-60)*S+30)*S**2)*D
STF AC2,(R0)+ ;SEND BACK
;COMPUTE A2
LDF FP60,AC2 ;60
MULF AC0,AC2 ;x S
SUBF FP90,AC2 ;- 90
MULF AC0,AC2 ;x S
ADDF FP30,AC2 ;+ 30
MULF AC0,AC2 ;x S
STF AC1,AC3
MULF AC1,AC3 ;D**2
MULF AC3,AC2 ;(((60*S-90)*S+30)*S)*D**2
STF AC2,(R0)+ ;SEND BACK
;COMPUTE A3
LDF FP60,AC2 ;60
MULF AC0,AC2 ;x S
SUBF FP60,AC2 ;- 60
MULF AC0,AC2 ;x S
ADDF FP10,AC2 ;+ 10
MULF AC1,AC3 ;D**3
MULF AC3,AC2 ;((60*S-60)*S+10)*D**3
STF AC2,(R0)+ ;SEND BACK
;COMPUTE A4
MULF FP30,AC0 ;30*S
SUBF FP15,AC0 ;- 15
MULF AC1,AC3 ;D**4
MULF AC3,AC0 ;(30*S-15)*D**4
STF AC0,(R0)+ ;SEND BACK
;COMPUTE A5
MULF FP6,AC3 ;6*D**4
MULF AC1,AC3 ;x D
STF AC3,(R0)+ ;SEND BACK
RTS PC ;RETURN
;END OF "MAK5TH"
;ATTSRV - ROUTINE FOR ATTACHING SERVOS
;"ATTSRV" FILLS THE DEVICE LIST WITH THE ADDRESSES OF THE SERVOS INDICATED
;BY THE SERVO BITS IN THE COEFFICIENT LIST HEADER. THE ADDRESS OF THE
;DEVICE LIST IS PLACED IN EACH SERVOS "INUSE" DATA BLOCK WORD TO INDICATE
;THAT THE SERVO IS ATTACHED TO A DEVICE. IF "INUSE" IS NOT INITIALLY ZERO
;IN ANY SERVO DATA BLOCK THAT IS TO BE ATTACHED, AN ERROR RETURN IS TAKEN.
;"DATPT", "POLY", "FCI", "TTIME", "COUNT" AND THE "MODBTS" WORD IN EACH SERVO
;DATA BLOCK ARE ALSO INITIALIZED. A SAMPLE CALL FOLLOWS:
;
; MOV #CBLK,R0 ;POINTER TO INFORMATION BLOCK
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,ATTSRV
; BCS ERROR ;CHECK FOR ERRORS
;WHERE
; CBLK: COEF ;POINTER TO COEFFICIENT LIST HEADER
; CNT ;MAXIMUM FUNCTION TIME FOR TIME OUT
; WAIT ;SERVO START WAIT TIME
; SEG ;REL. PTR. TO START OF SEGMENT DATA, 0 IF NO POLYNOMIALS
;
;THE DEVICE BLOCK IS LEFT IN THE FOLLOWING FORM:
;
; DEVICE: .BYTE NUM,0 ;WHERE NUM IS THE NUMBER OF SERVOS ATTACHED
; 0 ;LEAVES A BLANK TO PUT THE EVENT NUMBER IN
; SERVO-1 ;PTRS TO SERVO DATA BLOCKS
; SERVO-2
; :
; SERVO-NUM
;
;AFTER EXECUTION, IF AN ATTACH ERROR OCCURRED, THE C BIT IS SET AND R0 CONTAINS
;THE ERROR CODE "CNTATT", OTHERWISE, R0 CONTAINS THE NUMBER OF DEVICES ATTACHED.
;DEFINITIONS:
SETCNT==2
SETSWT==4
RELSEG==6
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
ATTSRV: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP) ;SAVE DEVICE BLOCK ADDRESS
MOV (R0),R4 ;GET ADDRESS OF COEF. LIST
MOV (R4),R2 ;GET THE SERVO BITS TO DETERMINE # OF REQ. SRV.
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
CLR R5
TST R2 ;CHECK IF SERVO #1 REQUESTED
1$: BPL .+4 ;SKIP IF THIS SERVO NOT REQUESTED
INC R5 ;ELSE INCREMENT SERVO COUNT
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE 1$ ;REPEAT TILL ALL SERVO BITS ADDED
MUL #A5COEF-A0COEF+4,R5 ;COMPUTE OFFSET TO START OF DYNAMIC COEF.
MOV R5,SETFCI ;SAVE OFFSET TO CII AND CI COEFFICIENTS
MOV OPBITS(R4),SETMOD ;GET THE COMMAND MODE BITS
MOV #A5COEF,SETPLY ;GET INITIAL REL PTR TO POLY DATA
MOV RELSEG(R0),SETSEG ;GET REL. PTR TO START OF SEGMENT DATA
BEQ 2$ ;BRANCH IF NO POLYNOMIAL TO FOLLOW
ADD (R0),SETSEG ;CONVERT SEG PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETPLY ;CONVERT POLY PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETFCI ;CONVERT DYNA. COEF. PTR TO ABS. ADDR.
ADD #A0COEF,SETFCI
2$: ADD #4,R1 ;START STORING SERVO PTRS. IN HERE
MOV (R4),R2 ;GET THE SERVO BITS AGAIN
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
MOV #SERVOS,R4 ;ADDR. OF TABLE CONTAINING SERVO DATA BLK PTRS
MOV (R4)+,R5 ;GET THE POINTER TO THE SERVO DATA BLOCK
CLR @(SP) ;ZERO SERVO COUNTER
TST R2
ATTLP: BPL NXTSRV ;BRANCH IF THIS SERVO IS NOT TO BE USED
TST INUSE(R5) ;CHECK IF SERVO ATTACHED TO ANOTHER DEVICE
BEQ NOTIUS ;BRANCH IF NOT ALREADY ATTACHED
ATTERR: MOV #CNTATT,R0 ;SHOULDN'T BE ATTACHED, SIGNAL ERROR
SEC
BR ATTDNE
NOTIUS: TST (R5) ;CHECK STATUS WORD OF DEVICE
BNE ATTERR ;ALL STATUS BITS SHOULD BE OFF
MOV (SP),INUSE(R5) ;PUT DEVICE ADDR. IN SERVO BLOCK TO INDICATE
; SERVO IS ATTACHED
MOV SETCNT(R0),COUNT(R5) ;SET COUNT FOR MAXIMUM FUNCTION TIME
MOV SETSWT(R0),TTIME(R5) ;SET START WAIT TIME
MOV SETSEG,DATPT(R5) ;SET POINTER TO SEGMENT DATA
MOV SETMOD,MODBTS(R5) ;SET THE OPERATION MODE BITS
MOV SETPLY,POLY(R5) ;SET PTR TO POLYNOMIAL COEF
MOV SETFCI,FCI(R5) ;SET PTR TO DYNAMIC COEF.
ADD #24.,SETPLY ;INCREMENT PTRS TO NEXT BLOCK OF COEF
ADD #10,SETFCI
MOV R5,(R1)+ ;PUT SERVO DATA ADDR. IN DEVICE LIST
INCB @(SP) ;ADD 1 TO THE SERVO COUNT
NXTSRV: MOV (R4)+,R5 ;POINT TO NEXT SERVO DATA BLOCK
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE ATTLP ;REPEAT IF SOME SERVO BITS LEFT
MOV @(SP),R0 ;RETURN NUMBER OF SERVOS ATTACHED
CLC ;INDICATE NO ATTACH ERRORS
ATTDNE: MOV (SP)+,R1 ;RESTORE REGISTERS
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC ;NORMAL RETURN
;LOCAL STORAGE AREA FOR "ATTSRV"
DATA
SETMOD: 0 ;OPERATION MODE BITS TO BE PUT IN MODBTS WORD
SETPLY: 0 ;POINTER TO START OF POLYNOMIAL COEF.
SETFCI: 0 ;POINTER TO DYNAMIC COEFFICIENTS, CII & CI
SETSEG: 0 ;POINTER TO SEGMENT DATA
CODE
;END OF "ATTSRV"
;RELSRV - ROUTINE FOR DETACHING SERVOS
;"RELSRV" RELEASES ALL SERVOS POINTED TO BY THE DEVICE BLOCK BY ZEROING
;INUSE WORDS AND STATUS WORD RUN ERROR FLAGS IN THE SERVO DATA BLOCKS. THE
;POINTERS TO THE ATTACHED SERVOS IN THE DEVICE BLOCK ARE REPLACED BY THE
;LOGICAL NUMBER OF THE SERVO AND ITS ASSOCIATED RUN ERROR BITS. THE SERVO
;NUMBER IS PLACED IN THE LOWER BYTE AND THE ERROR BITS IN THE UPPER BYTE OF
;THE DEVICE BLOCK WORD. ALL OF THE SERVO RUN ERROR BITS ARE OR'ED WITH
;REGISTER R0. A SAMPLE CALL FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,RELSRV
; TST R0 ;CHECK FOR RUN ERROR
;
;AFTER EXECUTION, THE FIRST WORD OF THE DEVICE BLOCK CONTAINS THE NUMBER
;OF SERVOS INITIALLY ATTACHED.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND R0 IS ALTERED
RELSRV: MOV R3,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R1,-(SP)
MOV R0,-(SP)
MOVB (R1),R3 ;GET THE NUMBER OF SERVOS ATTACHED
MOV R3,@2(SP) ;LEAVE NUMBER IN FIRST WORD OF DEVICE BLK
ADD #4,R1 ;SERVO POINTERS START HERE
TST R3 ;CHECK IF ANY SERVOS LEFT TO DETACH
1$: BLE 2$ ;BRANCH IF END OF DEVICE LIST
MOV (R1),R2 ;GET THE ADDR. OF A SERVO DATA BLOCK
BIS (R2),R0 ;OR ALL RUN ERROR BITS TOGETHER
MOVB SRVNUM(R2),(R1)+ ;SEND BACK SERVO NUMBER
MOVB 1(R2),(R1)+ ;SEND BACK RUN ERROR BITS
JSR PC,SETSET ;RESET ALL JOINT VARIABLES
DEC R3 ;DECREMENT NUMBER OF SERVOS LEFT TO DETACH
BR 1$ ;GO CHECK IF MORE SERVOS TO DO
2$: BIC #377,R0 ;ISOLATE SERVO RUN ERROR BITS
BIS (SP)+,R0 ;OR ON PERVIOUS ERROR BITS
MOV (SP)+,R1 ;RESTORE DEVICE BLOCK POINTER
MOV (SP)+,R2
MOV (SP)+,R3
RTS PC ;RETURN
;END OF "RELSRV"
;SETSET - RESETS SERVO BLOCK VARIABLES AFTER A MOVE IS COMPLETED
SETSET: CLRF THD(R2) ;SET ALL VELOCITIES TO ZERO
CLRF THDP(R2)
MOV TH(R2),THP(R2) ;SET PREDICATED JOINT ANGLE TO ACTUAL
MOV TH+2(R2),THP+2(R2)
CLRF DELTH(R2) ;NO DEVIATION FROM TRAJECTORY AT END
CLRF DELTHD(R2) ;NO FINAL CONSTANT VELOCITY
CLRF ERRINT(R2) ;ZERO INTEGRAL OF POSITION ERROR
CLRF POSERR(R2) ;NO FINAL POSITION ERROR
CLRF ERRTQE(R2) ;ZERO ERROR TORQUE
CLR INUSE(R2) ;CLEAR IN USE FLAG, DETACH SERVO
CLRB DACVAL(R2) ;ZERO DAC OUTPUT
BIC #-1#NONEX,(R2) ;CLEAR STATUS WORD ERROR BITS
BIC #NNUL+DEPTPT+WOBBLE,MODBTS(R2) ;RESET MODE WORD
RTS PC
;END OF "SETSET"
;SETTH - READS A/D AND INITIALIZES JOINT ANGLES FOR A GIVEN DEVICE BLOCK
;LEVEL 7 CODE IS INITIATED TO READ THE REQUIRED JOINT ANGLES FROM THE ADC.
;THE LEVEL 7 CODE IS REPEATED UNTIL ALL OF THE WIPERS ARE WITH IN OPERATING
;RANGE OR UNTIL 4 ATTEMPTS HAVE BEEN MADE, WHICHEVER OCCURS FIRST. IF AT
;THE END SOME WIPERS ARE STILL OUT OF RANGE, AN ERROR RETURN IS TAKEN. "TH",
;THE ACTUAL JOINT ANGLE, AND "THP", THE PREDICTED JOINT ANGLE ARE INITIALIZED
;TO THE SAME VALUE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETTH
; BCS ERROR ;BRANCH IF ERROR DURING SETTH
;
;IF A WIPER ERROR OCCURS, THE C BIT IS SET AND R0 CONTAINS THE ERROR CODE
;"BADWIP" WHEN THIS ROUTINE EXITS. IF NO WIPER ERROR OCCURRED, NEW DYNAMIC
;COEFFICIENTS ARE COMPUTED FOR ANY ARM THAT HAD ANY OF ITS JOINT ANGLES
;UPDATED.
;REGISTERS USED:
; R1 PASSES ARGUMENTS AND IS NOT MODIFIED
; R0, AC0 ARE GARBAGED
SETTH: MOV R1,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC STTLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC STTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #STTSRV,R2 ;PUT LIST OF SERVOS WITH POTS TO READ IN HERE
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
ADD #4,R1 ;SERVO POINTERS START HERE
CHKPOT: MOV (R1)+,R0 ;GET POINTER TO SERVO DATA BLOCK
TST POTCHN(R0) ;CHECK IF SERVO HAS A POT TO READ
BMI 1$ ;SKIP IF NO POT
MOV R0,(R2)+ ;SAVE SERVO POINTER
1$: SOB R4,CHKPOT ;GO CHECK NEXT POT
CLR (R2) ;INDICATE END OF SERVO LIST
TST STTSRV ;CHECK IF ANY POTS TO READ
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
MOV #4,STTCNT ;EXECUTE LEVEL7 CODE AT MOST 4 TIMES
.IFZ SNGSTP
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),STTEVT ;SAVE THE EVENT NUMBER
SCHED7 #STTPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
.IFF
JSR PC,STHLV7 ;EXECUTE LEVEL SEVEN CODE
.ENDC
TST STTWIP ;CHECK IF ALL WIPERS IN RANGE
BEQ 2$ ;BRANCH IF WIPERS OK
SEC ;INDICATE ERROR
MOV #BADWIP,R0
BR STTERR ;EXIT CLEANLY
;CONVERT THE A/D READINGS TO JOINT ANGLES FOR ALL SERVOS WITH POTS
2$: MOV #STTSRV,R4 ;SET POINTER TO LIST OF SERVOS WITH POTS
CLRB -(SP) ;COLLECT SERVO MECHANISM BITS IN HERE
STTLP: MOV (R4)+,R5 ;GET POINTER TO SERVO BLOCK DATA
BEQ COMPCI ;BRANCH IF END OF LIST
BISB MECHSM(R5),(SP) ;OR ALL MECHANISM BITS TOGETHER
MOV POT(R5),R0 ;GET THE POT READING
ADD #4000,R0 ;SHIFT POT READING TO 0→7777
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT AND CONVERT TO FLOATING POINT
.IFZ ISLIN
MOV R0,R3 ;SAVE NORMALIZED POT READING
CLR R1
ASHC #-10,R0 ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR R1 ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD R5,R0 ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(R0),R2 ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(R0),R0 ;GET UPPER LIMIT TO INTERPOLATE
SUB R2,R0 ;GET DIFFERENCE
MUL R1,R0 ;INTERPOLATE
ASHC #1,R0 ;GET 16 MSB
ADD R2,R0 ;NOW HAVE PROPER CALIBRATION
ADD R3,R0 ;ADD TO NORMALIZED POT READING
.ENDC
LDCIF R0,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(R5),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(R5),AC0 ;ADD POT OFFSET
STF AC0,TH(R5) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
STF AC0,THP(R5) ;SAVE AS PREDICTED JOINT ANGLE
BR STTLP ;GO DO NEXT SERVO
;RECOMPUTE DYNAMIC COEFFICIENTS IF ANY ARM JOINT ANGLES READ
COMPCI: MOVB (SP)+,R3 ;GET MECHANISM BITS FOR ALL SERVOS UPDATED
;PERHAPS NEXT INSTRUCITON SHOULD BE MOV #YELARM,R2 TO SAY YELLOW ARM TO DTERMS...*K
CLR R2 ;CHECK IF YELLOW ARM JOINT ANGLES UPDATED
BIT #YELARM,R3
BEQ 1$ ;BRANCH IF NOT
MOV #YTH,R0 ;ELSE COMPUTE DYNAMIC COEF, BASED UPON JT. ANGS.
MOV #YNCI,R1 ;SAVE IN SERVO DATA BLOCKS
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
1$: BIT #BLUARM,R3 ;CHECK IF BLUE ARM JOINT ANGLES UPDATED
BEQ STTDNE ;BRANCH IF NOT
MOV #BTH,R0 ;ELSE COMPUTE CURRENT DYNAMIC COEF
MOV #BNCI,R1 ;UPDATE SERVO DATA BLOCKS
INC R2 ;INDICATE BLUE ARM *K MOV #BLUARM,R2
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;EXIT CLEANLY
STTDNE: CLC ;INDICATE NO ERROR
STTERR: DEC STTLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC
;LOCAL STORAGE AREA
DATA
STTLCK: -1 ;-1 → SETTH READY FOR USE, 0 → LOCKED UP
STTEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
STTCNT: 4 ;NUMBER OF PASSES PERMITTED THROUGH LEVEL 7 CODE
STTWIP: 0 ;WIPER ERROR CODE CORRESPONDING TO "OUTRGE"
STTSRV: .BLKW 37. ;LIST OF SERVOS WITH POTS
STTPPB: STTPDB ;ARRAY INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
STTPDB: PDBLK7 STHLV7,STTSRV,20
CODE
;END OF "SETTH"
;SETREF - READ A/D, SET REFERENCE VOLTAGE AND ZERO TACH VALUE READINGS
;LEVEL 7 CODE IS INITIATED TO READ THE REFERENCE VOLTAGE SUPPLY AND ANY
;TACHOMETER CHANNELS THAT MAY EXIST FOR THE SERVOS TO BE USED. A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETREF
;
;THE REFERENCE VOLTAGE READING AND THE TACHOMETER ZERO READINGS ARE READ
;AND AVERAGED FOUR TIMES TO REDUCE THE NOISE LEVEL. IF ANY OF THE READINGS
;DEVIATES SIGNIFICANTLY FROM THE PREVIOUS AVERAGE, AN ERROR CODE IS RETURNED
;IN R0, OTHERWISE R0 IS SET TO ZERO.
;REGISTERS USED:
; R1 PASSES ARGUMENT AND IS UNALTERED
; R0 RETURNS AN ERROR CODE
SETREF: MOV R1,-(SP) ;SAVE REGISTERS AND PTR TO DEVICE BLOCK
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC SRFLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC SRFLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #SRFSRV+4,R2 ;SET POINTER TO LIST OF SERVOS WITH TACHS,SKIP
; OVER POWER SUPPLY REFERENCE READINGS
MOV #SRFADL+4,R3 ;SET POINTER TO A/D LIST, SKIP OVER REF. CHANNELS
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ ALLTAC ;BRANCH IF NO SERVOS ATTACHED
ADD #4,R1 ;POINT TO FIRST SERVO DATA BLOCK PTR IN DEVICE BLK
CHKTAC: MOV (R1)+,R0 ;GET PTR TO SERVO DATA BLOCK
TST TACCHN(R0) ;CHECK IF SERVO HAS A TACHOMETER
BMI 1$ ;SKIP IF NO TACH
MOV R0,(R2)+ ;SAVE SERVO POINTER
MOV TACCHN(R0),(R3)+ ;SAVE POT CHANNEL IN A/D LIST
MOV TACHZ0(R0),TACH(R0) ;SAVE PREVIOUS TACH ZERO READING
1$: SOB R4,CHKTAC ;REPEAT IF ANY SERVOS LEFT
ALLTAC: MOV #-1,(R3) ;INDICATE END OF A/D LIST
CLR (R2) ;INDICATE END OF SERVO LIST
MOV #4,SRFCNT ;AVERAGE ADC READINGS FOUR TIMES
.IFZ SNGSTP
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),SRFEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #SRFPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
.IFF
JSR PC,SRFLV7 ;EXECUTE LEVEL 7 CODE IN DIAGNOSTIC MODE
.ENDC
;SECTION TO CHECK IF READINGS WITHIN TOLERANCE, START WITH REFERENCE
MOV REFVT1,R0 ;GET NEW REFERENCE VOLTAGE READINGS
SUB REFER1,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BLE 1$ ;BRANCH IF REFERENCE OUT OF RANGE
MOV REFVT2,R0 ;REPEAT FOR SECOND REFERENCE READING
SUB REFER2,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BGT 2$ ;BRANCH IF REFERENCE IN OF RANGE
1$: MOV #BADREF,R0 ;ELSE SIGNAL ERROR
SEC
BR SEFDNE ;EXIT
2$: MOV #SRFSRV+4,R3 ;GET POINTERS TO SERVOS WITH TACHS
3$: MOV (R3)+,R4 ;GET SERVO DATA BLOCK ADDRESS
BEQ TACSOK ;BRANCH IF END OF LIST
MOV TACHZ0(R4),R0 ;GET TACH. ZERO VELOCITY READING
SUB TACH(R4),R0 ;COMPUTE DIFF. FROM PREVIOUS VALUE
BPL .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #10,R0 ;CHECK IF WITH LIMITS
BGE 3$ ;GO CHECK NEXT ONE IF OK
MOV #BADTAC,R0 ;ELSE SIGNAL ERROR
SEC
BR .+4 ;EXIT
TACSOK: CLC ;SIGNAL NO ERROR
;SECTION TO EXIT FROM "SETREF"
SEFDNE: DEC SRFLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC ;RETURN
;LOCAL STORAGE AREA
DATA
SRFLCK: -1 ;-1 → SETREF READY FOR USE, 0 → LOCKED UP
SRFCNT: .BLKW 1 ;EXECUTION COUNTER FOR LEVEL 7 CODE
SRFEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
SRFSRV: REFVT1-TACHZ0 ;STORAGE LOCATION FOR REF. POWER SUPPLY READINGS
REFVT2-TACHZ0
.BLKW 37. ;LIST OF SERVOS WITH TACHOMETERS
SRFADL: REFCN1 ;REFERENCE VOLTAGE SUPPLY A/D CHANNELS
REFCN2
.BLKW 37. ;A/D CHANNEL LIST
SRFVAL: .BLKW 38. ;A/D READINGS
SRFPPB: SRFPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST MARKER
SRFPDB: PDBLK7 SRFLV7,SRFSRV,20
CODE
;END OF "SETREF"
;STHLV7 & SRFLV7 - LEVEL 7 CODE FOR "SETTH" AND "SETREF"
;STHLV7 IS USED BY "SETTH" TO READ THE ADC POT CHANNELS. THIS ROUTINE IS
;REPEATED UNTIL ALL POTS ARE READ WITHIN WIPER RANGE OR UNTIL "STTCNT" IS
;DECREMENTED TO ZERO. "SETTH" MUST BE SCHEDULED TO RUN AT LEVEL 7.
;IT IS ASSUMED THAT THE KERNEL PRELOADS REGISTER "DAT" WITH A POINTER
;TO A TABLE OF POINTERS TO SERVOS WITH POTS TO BE READ.
STHLV7:
.IFNZ SNGSTP
MOV #STTSRV,DAT ;SET POINTER TO SERVOS WITH POTS
.ENDC
CLR OUTRGE ;ASSUME ALL WIPERS IN RANGE
MOV DAT,BC ;SAVE POINTER TO SERVOS WITH POTS
MOV (BC)+,DAT ;SET POINTER TO FIRST SERVO DATA BLOCK
MOVB POTCHN(DAT),ADCCHN ;START ADC WORKING ON FIRST CHANNEL
1$: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,POT(DAT) ;SAVE ADC POT READING
MOV (BC)+,CC ;GET POINTER TO NEXT SERVO DATA BLOCK
BEQ .+10 ;SKIP IF END OF LIST
MOVB POTCHN(CC),ADCCHN ;START CONVERTING NEXT CHANNEL
JSR PC,WIPER ;CHECK IF WIPER WITHIN OPERATING RANGE
MOV CC,DAT ;REPEAT TILL ALL SERVOS DONE
BNE 1$
DEC STTCNT ;HAS THIS CODE RUN 4 TIMES ALREADY?
BLE 2$ ;BRANCH IF NO MORE CHANCES TO GET WIPERS IN RANGE
TST OUTRGE ;ELSE TEST IF ANY WIPERS OUT OF RANGE
BEQ 2$ ;WE ARE DONE IF NO WIPERS OUT OF RANGE
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE THIS CODE TO RUN AGAIN
TST (SP)+ ;CLEAR SCHEDULED TIME OFF STACK
DISMS7 ;GO TO SLEEP
.ENDC
2$: MOV OUTRGE,STTWIP ;SAVE WIPER ERROR CODE
.IFZ SNGSTP
EVSIG STTEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
.IFF
RTS PC ;RETURN IF IN DIAGNOSTIC MODE
.ENDC
;SRFLV7 IS CALLED BY "SETREF" TO READ THE ADC TACHOMETER CHANNELS AND THE
;REFERENCE POWER SUPPLY VOLTAGE. THIS ROUTINE RESCHEDULES ITSELF TO AVERAGE
;THE READINGS "SRFCNT" NUMBER OF TIMES.
SRFLV7:
.IFNZ SNGSTP
MOV #SRFSRV,DAT ;SET POINTER TO SERVOS WITH TACHOMETERS
.ENDC
MOV #SRFADL,DC
MOV #SRFVAL,EC ;SET POINTER TO A/D READINGS
MOV DAT,CC
1$: MOV (CC)+,DAT ;SET POINTER TO SERVO BLOCK DATA
BEQ 2$ ;BRANCH IF END OF LIST
MOV (DC)+,AC
MOVB AC,ADCCHN
TSTB ADCSR
BPL .-4
MOV ADCVAL,AC
SUB TACHZ0(DAT),AC
ASR AC ;GET A 1/2 OF THE DIFFERENCE
ADD AC,TACHZ0(DAT) ;USE THIS TO CORRECT THE OLD READING
BR 1$ ;DO ALL SERVOS WITH TACHS
2$: DEC SRFCNT ;HAVE WE FINISHED AVERAGING YET?
BLE 3$ ;BRANCH IF WE ARE ALL THROUGH
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE THIS CODE TO RUN AGAIN
TST (SP)+ ;CLEAR SCHEDULED TIME OFF STACK
DISMS7 ;GO TO SLEEP
.ENDC
3$:
.IFZ SNGSTP
EVSIG SRFEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
.IFF
RTS PC ;RETURN IF IN DIAGNOSTIC MODE
.ENDC
;RUNSRV - SCHEDULES THE SERVOS OF A DEVICE TO RUN AND WAITS FOR THEIR COMPLETION
;THE FOLLOWING FUNCTIONS ARE PERFORMED BY THIS ROUTINE:
;
; 1) THE HARDWARE ERROR RESET IS TOGGLED AND A CHECK IS MADE TO
; ENSURE THAT THE POWER SUPPLY IS ON. IF IT'S NOT, AN ERROR FLAG
; IS RETURNED IN R0 OTHERWISE R0 IS CLEARED.
; 2) THE SERVOS LISTED IN THE DEVICE BLOCK ARE SCHEDULED FOR EXECU-
; TION AT LEVEL 7 ALONG WITH A DAC JOB TO REFRESH THE DACS.
; 3) THE TIME THAT EACH SERVO IS SCHEDULED TO FIRST RUN IS SAVED IN
; "PTIME" WITHIN EACH SERVO DATA BLOCK.
; 4) THE RUN BIT IS SET IN EACH SERVO STATUS WORD.
; 5) IF EITHER FORCE SENSING OR COMPLIANCE IS TO BE PERFORMED WITH
; THE ARM SPECIFIED BY THE SERVOS, THE FORCE SENSING LEVEL 7&5
; JOBS ARE STARTED AND SYNCHRONIZED TO THE ARM JOINTS. AN ERROR
; MESSAGE IS RETURNED AND THE FORCE SENSING IS ABORTED IF NOT
; ALL 6 OF THE ARM JOINTS ARE SCHEDULED TO RUN.
; 6) AFTER ALL SERVOS ARE COMPLETED, THIS ROUTINE KILLS THE DAC
; REFRESH JOB AND THE FORCE SENSING JOBS AND RETURNS TO THE
; CALLING ROUTINE.
;
;THIS SUBR IS CALLED WITH A PTR TO THE COEFFICIENT DATA LIST IN R0 AND
;A PTR TO THE DEVICE BLOCK IN R1
;REGISTERS USED:
; R1,R0 PASS ARGUMENT AND R0 IS ALTERED
RUNSRV: MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV (R1),-(SP) ;SAVE NUMBER OF SERVOS ATTACHED
MOV R1,-(SP) ;SAVE DEVICE BLOCK POINTER
MOV (R0),R3 ;GET SERVO BITS
.IFNZ TIMER
MOV #TIMES+2,TIMES ;RESET POINTER TO SAVE EXECUTION TIME DATA
.ENDC
.IFNZ FRCDAT
MOV #FDATA+2,FDATA ;RESET PTR TO FORCE DATA ARRAY
.ENDC
CLR R4 ;ASSUME NO FORCE SERVOING TO DO
TST GDATA ;IS DATA GATHER TO BE TURNED ON?
BEQ 4$
MOV #GATPBB,R4 ;SCHEDULE DATA GATHERER INSTEAD
BR REFRES ;NO OLD FORCE ROUTINES WHILE GATHERING
4$: TST FRCNUM ;ANY JOBS WAITING ON FORCE SENSING?
BNE 1$ ;YES
TST CMPCPN ;ANY FORCE COMPLIANCE TO DO?
BEQ REFRES ;NO
1$: BIT #YARM+BARM,R3 ;ARM BEING USED?
BEQ REFRES ;NO
MOV #BARM,R0 ;YES, ASSUME BLUE ARM
MOV #SRV13,R2
BIT #BLUARM,@FCARAY ;WERE WE RIGHT?
BNE .+12
MOV #YARM,R0 ;NO
MOV #SRV06,R2
BIC R3,R0 ;MAKE SURE ALL 6 JTS ASSIGNED
BEQ 2$ ;OK
MOV #FWRGJT,R0 ;SIGNAL ERROR
JMP RUNDNE
2$: MOV DATPT(R2),FDATPT;SAVE POLYNOMIAL DATA FOR FORCE RTNS
BNE 3$
MOV #NOPOLY,R0 ;SIGNAL CAN'T FORCE SERVO WITHOUT POLY
BR RUNDNE
3$: MOV POLY(R2),FPOLY
ADD #4,FPOLY
MOV INUSE(R2),FINUSE
MOV TTIME(R2),FTTME
MOV #FRCPBB,R4 ;SCHEDULE FORCE LEVEL 7 TO RUN
REFRES: BIT #BARM+BHAND,R3 ;BLUE ARM?
BEQ 1$ ;NO
INCB KICDAC ;NEED TO SCHEDULE DAC REFRESH JOB?
BIT #KICRUN,KICDAC
BNE 1$ ;NO, STILL RUNNING
MOVB #10,174004 ;RESET BLUE ARM INTERFACE
MOV #NOPOWR,R0
BIT #6,174004 ;MOTOR POWER ON?
BNE DECDAC ;NO, SIGNAL ERROR
CLR BPANIC ;CLEAR BLUE ARM HARDWARE ERROR FLAG
SCHEDU #MUGDAC,#DACSER,#USRDM,#20
BIS #KICRUN,KICDAC
1$: EVMAK ;THIS EVENT SIGNALS SERVOS DONE
MOVB (R1),R0 ;GET THE NUMBER OF SERVOS TO SCHEDULE
TST (R1)+
MOV (SP),(R1)+ ;SAVE EVENT NUMBER IN DEVICE BLOCK
MOV #SCHBLK,R2 ;CREATE ARRAY OF PDB'S TO PASS TO MONITOR
2$: MOV (R1)+,(R2) ;GET A SERVO DATA BLOCK PTR
ADD #SPDBLK,(R2)+ ;POINT TO PDB OF SERVO
TST (R2)+ ;ALLOW ROOM FOR THE MON. TO RETURN THE SCH.TIME
SOB R0,2$ ;REPEAT FOR ALL ATTACHED SERVOS
MOV R4,(R2)+ ;THIS EITHER PUTS THE FORCE LEVEL 7 JOB IN
TST (R2)+ ; THE SCHEDULE LIST OR MARKS THE END
CLR (R2)
.IFZ SNGSTP
SCHED7 #SCHBLK,SCTIME ;SCHEDULE ALL LEVEL7 JOBS
.ENDC
MOV #SCHBLK,R2 ;SAVE TIMES THAT THE SERVOS WHERE SCH. FOR
MOVB 4(SP),R0 ;# OF SERVOS SCHEDULED
INITSV: MOV (R2)+,R1 ;PTR TO SERVO DATA BLOCK
BIS #RUN,-SPDBLK(R1) ;SET SERVO RUN BIT
MOV (R2)+,PTIME-SPDBLK(R1) ;SET START TIME OF SERVO
SOB R0,INITSV
CMP (R2)+,#FRCPBB ;SAVE FORCE LEVEL 7 TIME
BNE 1$
MOV (R2),FPTIME
MOV #RUN+FIRST,FSTAT;INDICATE FIRST PASS THROUGH
CLR FRCJTS
1$: EVWAIT (SP) ;WAIT FOR SERVOS TO FINISH
EVKIL ;DELETE EVENT NUMBER
TST BOVLCK ;CHECK FOR BOVER ON BLUE ARM
BEQ 2$
MOV #BOVERR,R0 ;INDICATE BACKGROUND JOB ERROR
2$: TST GDATA ;WAS GATHER ENABLED?
BEQ 3$ ;NO
ASR GTIME ;COMPUTE NPTS
MOV GIPTR,R1 ;GET INTEGER POINTER
MOV GDATA,(R1)+ ;SAVE CONTROL BITS FOR GRAPH.SAI
MOV GTIME,(R1)+ ;SAVE NPTS FOR GRAPH.SAI
MOV R1,@ISAV ;REPLACE CHANGED POINTERS
MOV GPTR,@FPSAV
CLR GDATA ;TURN OFF DATA GATHER
3$: BIT #BARM+BHAND,R3 ;BLUE ARM?
BEQ RUNDNE ;NO
DECDAC: DECB KICDAC ;STOP DAC REFRESH JOB
RUNDNE: MOV (SP)+,R1
MOV (SP)+,(R1)
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
RTS PC
;LOCAL STORAGE AREA
DATA
SCHBLK: .BLKW 33. ;LIST OF PDB'S OF SERVOS TO SCHEDULE
SCTIME: 10. ;TIME DELAY BEFORE FIRST SERVO SHOULD BE SCHED
CODE
;END OF "RUNSRV"
;DACSER - BLUE ARM DAC REFRESH LEVEL 4 ROUTINE AND ARM ERROR TRAP ROUTINE
DACSER: MOV #7,R0 ;NUMBER OF DACS TO REFRESH
MOV #BSRVOS,R1 ;BLUE ARM SERVO DATA BLKS
MOVB #20,DACCSR ;RESET HARDWARE TIME-OUT
1$: MOV (R1)+,R5 ;PTR TO DATA BLK
MOV #RUN+MOVING,R2 ;NEED TO REFRESH DAC?
BIC (R5),R2
BEQ 2$ ;NO
BIT #DACDNE,DACCSR ;DAC READY FOR NEW VALUE?
BEQ .-6 ;NO
MOV DACCHN(R5),DACCSR;REFRESH A DAC
2$: SOB R0,1$
TSTB KICDAC ;NEED TO KEEP RE-FRESHING?
BEQ 3$ ;NO
SLEEP #16.
BR DACSER
3$: BIC #KICRUN,KICDAC ;DISMISS FOREVER
BIS #10,174004 ;DISABLE BLUE ARM ALARM INTERRUPTS
DISMISS
;ARM ERROR INTERRUPT ROUTINE
ARMERR: BIS #1,BPANIC ;INDICATE BLUE ARM ERROR CONDITION
KRTI ;RETURN USING EMT IN KERNEL
YERR: BIS #1,BPANIC ;INDICATE YELLOW ARM ERROR CONDITION
.IFZ STDALN
ALERR YEMSG ;TELL OPERATOR, YELLOW PANIC BUTTON PUSHED
.IFF
MOV #YEMSG,SG
JSR PC,TYPSTR
.ENDC
KRTI ;RETURN VIA KERNEL
;LOCAL STORAGE AREA
DATA
BPANIC: 0 ;SET TO 1 IF PANIC BUTTON HIT
KICDAC: .WORD 0 ;DAC REFRESH STATUS BITS CONTAINS:
KICRUN==100000 ;RE-FRESH JOB RUNNING
; 377 ;≠0 MEANS DONT STOP RUNNING
YEMSG: .ASCIZ /YELLOW PANIC BUTTON PUSHED!/
MUGDAC: PDBLK 4,20
CODE
;END OF DAC REFRESH ROUTINE
; TOUCH - SCHEDULES TOUCH EVENTS
;THIS PROGRAM IS USED FOR SCHEDULING EVENTS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE STATE OF A TOUCH SENSOR. THE EVENT SIGNALING CAN TAKE PLACE WHEN A
;TOUCH SENSOR READS ON OR OFF. AS MANY AS TWENTY-FIVE DIFFERENT EVENTS CAN
;BE WAITING FOR VARIOUS TOUCH SENSOR STATES. READING OF THE TOUCH SENSORS AND
;DECIDING WHICH EVENTS ARE TO BE ACTIVITED IS DONE AT LEVEL 7. THE LEVEL 7
;JOB IS SCHEDULED ONLY WHEN SOME EVENTS ARE PENDING. THE WORDS CONTAINING THE
;STATUS OF EACH TOUCH SENSOR ARE UPDATED EACH TIME THE LEVEL 7 JOB IS RUN.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TRIGGERED ON TOUCH
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; NEED TO TRIGGER EVENT
; JSR PC,TOUCH
;
;IF THE HIGH BYTE OF THE SENSOR NUMBER IS ZERO, THE OFF CONDITION WILL TRIGGER
;THE EVENT, ELSE THE ON CONDITION OF THE SENSOR WILL BE USED AS A TRIGGER.
;THE LOW BYTE OF THE "SENSOR" NUMBER MUST CONTAIN THE LOGICAL NUMBER FOR THE
;SENSOR TO BE TESTED. THE LOGICAL NUMBERS ASSIGNMENTS ARE AS FOLLOWS:
;
; SENSOR LOGICAL NUMBER
; BLUE HAND, RIGHT SIDE 0
; BLUE HAND, LEFT SIDE 1
; YELLOW HAND, RIGHT SIDE 2
; YELLOW HAND, LEFT SIDE 3
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR OCCURRED.
;THE ERROR CODES ARE LISTED ON PAGE 8.
;DEFINITIONS:
MAXTCH ==2 ;TOTAL NUMBER OF OPERATIONAL TOUCH SENSORS
TCHBLK ==25. ;NUMBER OF F.S. BLOCKS IN TOUCH LIST
TCHSCH ==400 ;LEVEL 7 STATUS BIT, INDICATES LEVEL 7 CODE SCHEDULED
RUNTCH ==1000 ; " " " " , ALWAYS RE-SCHEDULE LEVEL 7 CODE
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
TOUCH: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT TCHOK
TCHERR: MOV #UNKTCH,R0 ;SIGNAL ERROR
BR TCHDNE
TCHOK: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #177400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;PUT TOUCH EVENT IN EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 7 CODE FROM USING EVENT LIST
MOV LSTUSE,R3 ;START CHECKING FROM LAST SLOT USED
MOV #TCHBLK,R2 ;CHECK ALL EVENT SLOTS FOR A FREE ONE
1$: SUB #4,R3 ;CHECK NEXT SLOT
BGT .+6 ;WRAP AROUND IF AT BOTTOM OF LIST
MOV #TCHBLK*4-4,R3
TST EVTLST(R3) ;CHECK IF THIS EVENT SLOT FREE
BEQ 2$ ;BRANCH IF EMPTY
SOB R2,1$ ;CHECK ALL AVAILABLE SLOTS
MOV #TOMTCH,R0 ;NO EMPTY SLOTS, SIGNAL ERROR
BR 4$
2$: MOV R3,LSTUSE ;SAVE PTR TO LAST SLOT USED
ADD #EVTLST,R3 ;GET ABSOLUTE ADDRESS OF FREE SLOT
MOV R0,(R3) ;PUT IN EVENT TO BE TRIGGERED
MOV (R1),2(R3) ;SAVE PTR TO PREVIOUS EVENT TIED TO THIS
; TOUCH SENSOR CONDITION
MOV R3,(R1) ;SAVE PTR TO THIS EVENT
INC ACTNUM ;INCREASE THE NUMBER OF SLOTS NOW TAKEN
BIT #TCHSCH,TCHUP ;CHECK IF LEVEL 7 CODE SCHEDULED TO RUN
BNE 3$ ;BRANCH IF SCHEDULED
.IFZ SNGSTP
SCHED7 #TCHPPB,#1 ;SCHEDULE LEVEL 7 CODE TO READ TOUCH SENSORS
BIS #TCHSCH,TCHUP ;INDICATE LEVEL 7 CODE SCHEDULED
.ENDC
3$: CLR R0 ;INDICATE NO ERRORS
4$: DECB TCHUP ;ALLOW LEVEL 7 CODE TO USE EVENT LIST
;EXIT CLEANLY
TCHDNE: MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R2
RTS PC
;END OF "TOUCH"
;CLRTCH - TAKES A SCHEDULED EVENT OFF OF THE TOUCH SENSOR EVENT LISTS
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "TOUCH". A SAMPLE CALLING
;SEQUENCE TO "CLRTCH" FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TAKEN OFF LIST
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; WITH WHICH EVENT WAS ASSOCIATED
; JSR PC,CLRTCH
;
;THE INTERPRETATION OF THE "EVENT" AND "SENSOR" NUMBERS IS THE SAME AS IN
;"TOUCH". AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO
;ERROR OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8. IF NO TOUCH EVENT
;WAS FOUND, THE NORMAL ERROR RETURN IS TAKEN WITH NO ERROR INDICATED.
;REGISTERS USED:
;
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
CLRTCH: MOV R2,-(SP) ;SAVE REGISTER
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT 1$
MOV #UNKTCH,R0 ;SIGNAL ERROR
BR CTHDNE
1$: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;TAKE TOUCH EVENT OFF EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 7 CODE FROM USING EVENT LIST
2$: TST (R1) ;END OF LIST?
BEQ CTEOL ;BRANCH IF NO EVENT MATCH FOUND
MOV (R1),R2 ;ELSE GET POINTER TO EVENT
CMP R0,(R2)+ ;MATCHING EVENT?
BEQ GOTEVT ;BRANCH IF WE FOUND IT
MOV R2,R1 ;ELSE GET POINTER TO NEXT EVENT ON LIST
BR 2$ ;CHECK NEXT EVENT
GOTEVT: MOV (R2),(R1) ;SKIP OVER REQUESTED EVENT
CLR -2(R2) ;FREE EVENT SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
CTEOL: DECB TCHUP ;UNLOCK LEVEL 7 CODE
CLR R0 ;INDICATE NO ERRORS
;EXIT CLEANLY
CTHDNE: MOV (SP)+,R2 ;RESTORE REGISTER
RTS PC
;END OF "CLRTCH"
;SMPTCH,STPTCH - INITIATES AND TERMINATES UPDATING OF THE TOUCH SENSORS READINGS
;"SMPTCH" ENSURES THAT THE TOUCH SENSOR STATUS WORDS WILL BE UPDATED PERIODLY
;WHETHER OR NOT ANY TOUCH SENSOR EVENTS ARE PENDING. A SAMPLE CALLING SEQUENCE
;TO THIS ROUTINE FOLLOWS:
;
; JSR PC,SMPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
SMPTCH: INCB TCHUP ;LOCKOUT LEVEL 7 CODE
BIS #RUNTCH,TCHUP ;TELL LEVEL 7 CODE TO RUN CONTINUOUSLY
BIT #TCHSCH,TCHUP ;CHECK IF LEVEL 7 CODE SCHEDULED TO RUN
BEQ 1$ ;BRANCH IF SCHEDULED
.IFZ SNGSTP
SCHED7 #TCHPPB,#1 ;SCHEDULE LEVEL 7 CODE TO READ TOUCH SENSORS
BIS #TCHSCH,TCHUP ;INDICATE LEVEL 7 CODE SCHEDULED
.ENDC
1$: DECB TCHUP ;UNLOCK LEVEL 7 CODE
RTS PC
;"STPTCH" RETURNS THE LEVEL 7 CODE TO MODE OF OPERATION OF ONLY RUNNING IF
;THERE IS A PENDING TOUCH SENSOR EVENT IN THE EVENT LIST. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; JSR PC,STPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
STPTCH: BIC #RUNTCH,TCHUP ;RETURN TO STANDARD MODE
RTS PC
;END OF "SMPTCH" AND "STPTCH"
;TCHLV7 - LEVEL 7 CODE FOR TOUCH SENSORS
;THIS ROUTINE UPDATES THE STATUS WORDS FOR THE TOUCH SENSORS ( TCHSTS ) WHEN
;IT IS RUN SO LONG AS THE LOCKOUT BYTE OF THE STATUS WORD ( TCHUP ) IS ZERO.
;IF UPDATING IS PERMITTED, ANY EVENTS IN THE EVENT LIST WHOSE CONDITION IS
;NOW TRUE ARE SIGNALLED. THIS ROUTINE RE-SCHEDULES ITSELF IF AT LEAST
;ONE EVENT IS PENDING OR IF THE "RUNTCH" BIT OF THE "TCHUP" WORD IS ON.
;IF NOT LOCKED OUT, UPDATE SENSORS AND SIGNAL TRUE CONDITION EVENTS
TCHLV7: TSTB TCHUP ;CHECK IF LOCKED OUT
BNE SCHTCH ;LOCKED OUT, JUST GO RE-SCHEDULE TO RUN AGAIN
MOV #TCHADL,AC ;PTR TO ADC CHANNELS TO READ
MOV #TCHVAL,BC ;PTR TO ARRAY TO STORE ADC READINGS
MOV #TCHOFF,CC ;PTR TO "OFF" CONDITION EVENTS
MOV #TCHON,DC ;PTR TO "ON" CONDITION EVENTS
MOV #TCHSTS,EC ;PTR TO ON/OFF STATUS WORDS FOR CHANNELS
MOVB (AC)+,ADCCHN ;START ADC WORKING ON FIRST CHANNEL
MOV #TLVEXT,-(SP) ;SAVE ADDRESS OF EXIT SECTION
TLV7LP: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,(BC) ;SAVE ADC READING
MOVB (AC),ADCCHN ;START CONVERTING NEXT CHANNEL
CMP SENZ0-TCHVAL(BC),(BC)+ ;CHECK SENSOR VALUE
BLT 1$ ;BRANCH IF SENSOR ABOVE ZERO LEVEL
CLR (EC)+ ;INDICATE SENSOR OFF
MOV (CC),DAT ;WANT PTR TO SIGNAL "OFF" CONDITION EVENTS
CLR (CC)+ ;INDICATE NO MORE EVENTS PENDING ON THIS CONDITION
TST (DC)+ ;UPDATE "ON" EVENT POINTERS
BR 2$
1$: MOV #1,(EC)+ ;INDICATE SENSOR ON
MOV (DC),DAT ;WANT PTR TO "ON" CONDITION EVENTS
CLR (DC)+
TST (CC)+ ;UPDATE "OFF" EVENT POINTER
2$: TST DAT ;CHECK IF ANY EVENTS PENDING
BEQ NXTSEN ;IF NONE, SKIP TO NEXT SENSOR
3$: MOV (DAT),-(SP) ;WE WILL SIGNAL THIS LATER SINCE THE KERNEL WOULD
MOV #SGNEVT,-(SP) ; CLOBBER OUR REG. IF WE DID IT NOW
CLR (DAT)+ ;FREE EVENT LIST SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
MOV (DAT),DAT ;GET PTR TO NEXT EVENT ON LIST
BNE 3$ ;REPEAT FOR ALL EVENTS ATTACHED TO THIS CONDITION
NXTSEN: TSTB (AC)+ ;CHECK IF END OF ADC LIST
BPL TLV7LP ;REPEAT TILL DONE
BR TSTSIG
SGNEVT:
.IFZ SNGSTP
EVSIG ;SIGNAL AN EVENT THAT OCCURRED
.ENDC
TSTSIG: RTS PC ;THIS BRANCHS TO SGNEVT IF WE SAVED ANY EVENTS TO
TLVEXT: ; SIGNAL, OTHERWISE IT COMES HERE
TST ACTNUM ;CHECK IF ANY EVENTS STILL PENDING
BNE SCHTCH ;BRANCH IF STILL MORE
BIT #RUNTCH,TCHUP ;ELSE CHECK IF WE ARE TO RUN CONTINUOUSLY
BEQ TLVKIL ;IF NOT, NO MORE TO DO, KILL LEVEL 7 CODE
;RE-SCHEDULE TO RUN AGAIN
SCHTCH:
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE LEVEL 7 CODE TO READ TOUCH SEN.
TST (SP)+ ;CLEAR SCHEDULE TIME OFF STACK
DISMS7
.ENDC
;DON'T RE-SCHDULE
TLVKIL:
.IFZ SNGSTP
BIC #TCHSCH,TCHUP ;INDICATE LEVEL 7 NOT SCHEDULED AGAIN
DISMS7
.ENDC
;TOUCH SENSOR SERVO DATA BLOCK
DATA
TCHUP: 0 ;LEVEL 7 STATUS BITS
ACTNUM: 0 ;NUMBER OF PENDING EVENTS
TCHADL: .BYTE 0,1,377 ;TOUCH SENSOR A/D CHANNELS
.EVEN
SENZ0: 450. ;TRANSITION READING FOR SENSORS
450.
;TCHVAL TO LSTUSE IS ZEROED BY "INTARM"
TCHVAL: .BLKW MAXTCH ;A/D TOUCH SENSOR READINGS
TCHSTS: .BLKW MAXTCH ;TOUCH SENSOR LOGICAL STATUS ( TRUE OR FALSE )
TCHON: .BLKW MAXTCH ;PTR TO EVENTS TO TRIGGER IF TOUCH SENSOR ON
TCHOFF: .BLKW MAXTCH ; " " " " " " " " OFF
EVTLST: .BLKW TCHBLK*2;STORAGE FOR EVENTS TO BE TRIGGERED
LSTUSE: TCHBLK*4 ;REL. POINTER TO LAST SLOT USED IN EVENT LIST
;TOUCH SENSOR LEVEL 7 CODE PROCESS DESCRIPTOR BLOCK
TCHPPB: TCHPBB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
TCHPBB:
.IFZ SNGSTP
PDBLK7 TCHLV7,0,20
.ENDC
CODE
;END OF "TCHLV7"
; FORCE - SUBRS. USED FOR FORCE SENSING AND COMPLIANCE
FRATE ==3 ;NUMBER OF TIMES FRCLV7 IS EXECUTED BEFORE A CALL TO FBACK
WRSPLS ==40 ;FORCE WRIST PULSE BIT, TO RESET MULTIPLEXER
WRSCHN ==66 ;FORCE WRIST ADC CHANNEL
FRCBLK ==15. ;NUMBER OF FORCE JOB THAT CAN BE PENDING
;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
;THE FOLLOWING BITS ARE USED DURING CALLS TO THE FORCE SENSING SYSTEM AND
;BY THE FORCE SENSING SYSTEM ITSELF TO SPECIFY RUN-TIME OPTIONS.
FTABLE ==400 ;FORCE TRANS (C) IN TABLE COORDINATES
FHAND ==0 ; " " " " HAND COORDINATE SYSTEM
XFORCE ==0 ;FORCE IN X DIRECTION OF C
YFORCE ==1000 ; " " Y " " "
ZFORCE ==2000 ; " " Z " " "
XMOMENT ==3000 ;MOMENT ABOUT X DIRECTION OF C
YMOMENT ==4000 ; " " Y " " "
ZMOMENT ==5000 ; " " Z " " "
FSTOP ==10000 ;IN ADDITION TO SPROUTING JOB, STOP ARM
SIGGE ==100000;START JOB IF FORCE ≥ SPECIFIED VALUE
SIGLT ==0 ; " " " " < " "
;SUBR TO VERIFY SAME ARM SPECIFIED BY FORCE ROUTINES
SAMARM: MOV R0,R3 ;SAME ARM SPECIFIED?
BIC #-1#BLUARM#YELARM,R3
BIT R3,@FCARAY
BNE 1$
MOV #WRGARM,R0 ;NO, SIGNAL ERROR
SEV
1$: RTS PC
;SETC - INITIALIZES FORCE SENSING/COMPLIANCE SYSTEM
.IFZ DIAGY
;THIS ROUTINE IS USED FOR SPECIFYING THE FORCE COORDINATE SYSTEM TOGETHER
;WITH THE MANIPULATOR THAT IS TO BE FORCE SERVOED AND FOR INITIALIZING
;THE FORCE SENSING/COMPLIANCE ROUTINES. IT SHOULD BE CALLED AT LEAST
;ONCE BEFORE THE START OF EACH MOTION THAT REQUIRES FORCE SENSING/COMPLIANCE.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;BLUARM/YELARM AND TABLE/HAND BITS
; MOV #C,R1 ;PTR TO FORCE TRANS
; JSR PC,SETC
;
;IF THE FORCE SENSING SYSTEM IS CURRENTLY ACTIVE AND THE ARM SPECIFIED
;IN R0 IS DIFFERENT FROM THE ARM CURRENTLY IN USE, THIS INSTRUCTION IS
;ABORTED AND AN ERROR CODE IS RETURNED IN R0, ELSE R0 IS SET TO ZERO.
;**NOTE** IF THE FORCE SENSING SYSTEM IS NOT ACTIVE AT THE TIME THAT
;THIS ROUTINE IS CALLED, THE ACTION OF ALL PREVIOUS "COMPLY" COMMANDS
;IS UN-DONE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE GARBAGED
SETC: MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
BIT #RUN,FSTAT ;FORCE SYSTEM ACTIVE?
BEQ 1$ ;NO
JSR PC,SAMARM ;YES, SAME ARM?
BVS SETCDN ;NO
BR 5$
1$: CLR FRCJTS ;SIGNAL NO JTS FORCE SERVOING
CLR CMPCPN ;FORCE COMPLIANCE ONLY GOOD ONCE
MOV #OTCN,R2 ;INITIALIZE FORCE TRANS
MOV #TCP2,R3
MOV #IMAT,R4
MOV #12.*2,R5
2$: MOV (R4),(R2)+
MOV (R4),TTRAN-IMAT(R4)
MOV (R4)+,(R3)+
SOB R5,2$
CLRF OP6
CLRF OP6+4
CLRF OP6+10
MOV #TTHC,TTHCP ;SET ANGLE ARRAY PTR
MOV #TTH,TTHP
MOV #SERVOS,R2 ;PTRS TO SERVO DATA BLOCKS
BIT #BLUARM,R0 ;BLUE ARM?
BEQ 3$
ADD #7*2,R2 ;YES
SUB #14.,TTHCP
SUB #14.,TTHP
3$: MOV R2,SRVPTR ;SAVE PTR
MOV #TTHC1,R4 ;INITIALIZE ANGLES TO CURRENT VALUES
MOV #6,R5
4$: MOV (R2)+,R3 ;SERVO DATA BLOCK
MOV TH(R3),(R4)+ ;TRANSFER CURRENT JOINT ANGLE
MOV TH+2(R3),(R4)+
SOB R5,4$
5$: MOV FCARAY,R2 ;PTR TO CURRENT C TRANS AND BITS
MOV NEXTC(R2),R2 ;PTR TO NEXT TRANS/BITS DATA AREA
MOV R2,R3
MOV R0,(R2)+ ;SAVE ARM/MODE BITS
MOV #12.*2,R0 ;TRANSFER C TRANSFORM
MOV (R1)+,(R2)+
SOB R0,.-2
MOV R3,FCARAY ;START USING NEW C ARRAY AND BITS
SETCDN: MOV (SP)+,R5
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "SETC"
;FRCSIG - INITIALIZES JOB STARTING BASED UPON A FORCE READING
;THIS RTN. IS USED FOR SPROUTING JOBS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE VALUE OF A FORCE COMPONENT. THE SPROUTING CAN TAKE PLACE WHEN
;A FORCE LEVEL EITHER EXCEEDS OR DROPS BELOW A SPECIFIED MAGNITUDE. THE ONLY
;CONSTRAINTS ARE THAT THE FORCE COMPONENT TO BE CHECKED MUST COINCIDE WITH
;ONE OF THE CARDINAL AXES OF THE CURRENT FORCE TRANSFORM( SEE "SETC") AND THE
;ARM SPECIFIED MUST BE THE SAME AS THAT SPECIFIED IN THE LAST "SETC" CALL. AS
;MANY AS "FRCBLK" DIFFERENT JOBS CAN BE WAITING FOR VARIOUS FORCE LEVELS. THE
;EVALUATION OF THE CURRENT FORCE LEVELS ARE DONE AT LEVEL 7 AND A SUBSIDIARY
;JOB IS RUN AT LEVEL 5. BOTH OF THESE JOBS ARE RUN ONLY WHEN SPROUTING IS
;PENDING OR SOME FORCE COMPLIANCE TASK IS REQUESTED.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; MOV #PDB,R1 ;PDB OF JOB TO BE TRIGGERED ON FORCE
; MOV #ADDR,R2 ;STARTING ADDR OF SPROUTING JOB
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,FRCSIG ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO CHECK: FORCE X/Y/Z MOMENT X/Y/Z
; START JOB IF: FORCE ≥/< SPECIFIED VALUE
; STOP ARM WHEN JOB STARTED: YES/NO
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;QUEUING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,R1,R2,AC0 PASS ARGUMENTS AND R0 IS ALTERED
FRCSIG: MOV R3,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS FRCDNE ;NO
MOV R0,R3 ;GET PTR INTO FORCE COMPONENT LIST
BIC #170777,R3
SWAB R3
;PUT FORCE JOB IN QUEUE
INCB FSTAT+1 ;LOCK OUT LEVEL 7 CODE FROM USING JOB LIST
MOV FRCPTR,R4 ;ANY FREE JOB BLOCKS?
BNE 1$ ;YES
MOV #TOMFRC,R0 ;NO, SIGNAL ERROR AND ABORT
BR 2$
1$: MOV (R4),FRCPTR ;TAKE BLOCK OUT OF FREE CHAIN
MOV XFPTR(R3),(R4) ;PUT BLOCK IN COMPONENT CHAIN
MOV R4,XFPTR(R3)
TST (R4)+ ;SAVE JOB DATA IN BLOCK
STF AC0,(R4)+ ;FORCE VALUE
MOV R0,(R4)+ ;BITS
MOV #3,(R4)+ ;TEST COUNTER
MOV R2,(R4)+ ;SPROUTING JOB
MOV R1,(R4)
INC FRCNUM ;INDICATE ONE MORE JOB PENDING
CLR R0 ;INDICATE NO ERRORS
2$: DECB FSTAT+1 ;ALLOW LEVEL 7 CODE TO USE JOB LIST
;EXIT CLEANLY
FRCDNE: MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
RTS PC
;END OF "FRCSIG"
;FRCOFF - TAKES A QUEUED JOB OFF OF THE FORCE SIGNAL LIST
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "FRCSIG". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME TWO ARGS AS IN "FRCSIG"
; MOV #PDB,R1
; JSR PC,FRCOFF
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
FRCOFF: MOV R3,-(SP) ;SAVE REGISTER
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 5$ ;NO
BIC #170777,R0 ;GET PTR INTO FORCE COMPONENT LIST
SWAB R0
ADD #XFPTR,R0 ;PTR TO SPECIFIED COMPONENT LIST
INCB FSTAT+1 ;LOCK OUT LEVEL 7 CODE FROM USING JOB LIST
MOV (R0),R3 ;FIRST JOB IN QUEUE
;TAKE FORCE JOB OUT OF QUEUE
1$: CMP 14(R3),R1 ;FOUND RIGHT PDB?
BEQ 3$ ;YES
MOV (R3),R3 ;NEXT JOB IN QUEUE
BNE 1$
MOV #1,R0 ;COULDN'T FIND REQUESTED JOB
BR 4$ ;NO MORE JOBS TO CHECK
3$: MOV (R3),(R0) ;REMOVE JOB FROM QUEUE
MOV FRCPTR,(R3)
MOV R3,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
CLR R0 ;SIGNAL NO ERROR
4$: DECB FSTAT+1 ;ALLOW LEVEL 7 CODE TO USE JOB LIST
5$: MOV (SP)+,R3
RTS PC
;END OF "FRCOFF"
;COMPLY - SETS UP FORCE COMPLIANCE OF A GIVEN MAGNITUDE AND DIRECTION
;THIS IS JUST LIKE "FRCSIG" EXCEPT THAT INSTEAD OF QUEUING JOBS WAITING
;ON FORCE LEVELS, THIS ROUTINE INITIALIZES FORCE COMPLIANCE IN A GIVEN
;CARDINAL DIRECTION OF THE CURRENT FORCE TRANSFORMATION ( SEE "SETC" )
;AND AT A GIVEN FORCE MAGNITUDE. AS WITH "FRCSIG" THE SPECIFIED ARM
;MOST COINCIDE WITH THE ARM SPECIFIED IN THE LAST CALL TO "SETC" OR
;AN ERROR MESSAGE IS RETURNED. NATURALLY, ONLY ONE COMPLIANCE LEVEL
;CAN BE SET IN ANY SINGLE DIRECTION AND SO ONLY THE MOST RECENT FORCE
;LEVELS ARE RETAINED. THIS ROUTINE SHARES A LEVEL 7 AND A SUBSIDIARY
;LEVEL 5 JOB WITH "FORSIG".
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,COMPLY ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO APPLY: FORCE X/Y/Z MOMENT X/Y/Z
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR
;OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,AC0 PASS ARGUMENTS AND R0,R1 ARE ALTERED
COMPLY: MOV R3,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
MOV R0,R3
ASL R0
STF AC0,CMPFRC(R0) ;SAVE COMPLIANCE LEVEL
ASR R3 ;GET BIT INDICATING WHICH DIRECTION
MOV #1,R1
ASH R3,R1
BISB R1,NEWCPN ;INDICATE POSSIBLE NEW COMPLIANCE COMPON.
CLR R0 ;INDICATE NO ERRORS
1$: MOV (SP)+,R3
RTS PC
;END OF "COMPLY"
;CMPOFF - TURNS OFF FORCE COMPLIANCE IN A SPECIFIED DIRECTION
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "COMPLY". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME ARG AS IN "COMPLY"
; JSR PC,CLRFRC
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0 PASSES ARGUMENT AND IS ALTERED
CMPOFF: MOV R3,-(SP) ;SAVE REGISTER
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$ ;NO
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
ASR R0 ;GET BIT INDICATING WHICH DIRECTION
MOV #1,R3
ASH R0,R3
BICB R3,NEWCPN ;TURN OFF COMPLIANCE IN THIS DIRECTION
BICB R3,CMPCPN
CLR R0 ;SIGNAL NO ERROR
1$: MOV (SP)+,R3
RTS PC
;END OF "CMPOFF"
;FRCLV7 - LEVEL 7 CODE FOR FORCE SENSING AND COMPLIANCE
;AT THE START OF EXECUTION OF THIS ROUTINE, DAT ← #XFPTR
FRCLV7: TST @FINUSE ;CATASTROPHIC ERROR TERMINATION?
BPL .+6
JMP KILFRC ;YES
TST FRCNUM ;ANY FORCE SENSING TO DO?
BEQ CHKDIE ;NO
BIT #177400+FIRST,FSTAT;1ST PASS OR LOCKED OUT FROM USING JOB LIST?
BNE RESFRC ;YES
MOV #ERRPTR,AC ;PTR TO TABLE OF PTRS TO JT ERROR TORQUES
BIT #BLUARM,@FCARAY ;NEED BLUE ARM?
BEQ .+6
ADD #14.,AC ;YES
MOV FJARAY,BC ;PTR TO JACOBIAN
MOV #6,EC ;# OF FORCE COMPONENTS TO CHECK
MOV #FRCEXT,-(SP) ;"RTS" THIS TO EXIT
FRCLOP: MOV (DAT),DC ;ANY JOBS PENDING ON THIS COMPONENT?
BEQ NOFJOB
TST <24.*6>(BC) ;SUM OF SQUARES COMPUTED?
BEQ NOFJOB
CLRF AC0
MOV #6,CC ;COMPUTE DOT PROD. OF ERR. TOR. AND JACOB
1$: LDF @(AC)+,AC1 ;ERROR TORQUE
MULF (BC),AC1 ;JACOBIAN COMPONENT
ADD #24.,BC
ADDF AC1,AC0 ;DOT PRODUCT
SOB CC,1$
DIVF (BC),AC0 ;DOT PROD/SUM OF SQRS
SUB #<24.*6>,BC ;RESET JACOBIAN & ERROR TRQ PTRS
SUB #<2*6>,AC
NEGF AC0
MOV DAT,CC ;CHECK IF ANY PENDING JOBS TO START UP
CHKJOB: TST (DC)+
CMPF (DC)+,AC0 ;CHECK VALUE
TST (DC)+ ;SPROUT ON FORCE ≥ VAL OR < VAL?
BPL 1$
CFCC ;VAL ≤ FORCE?
BLE 2$ ;YES
BR 5$ ;NO
1$: CFCC ;VAL > FORCE?
BLE 5$ ;NO
2$: DEC (DC) ;FILTER, DONT STOP UNLESS TEST OK 3 TIMES
BGT 6$
BIT #FSTOP,-(DC) ;STOP ARM MOTION?
BEQ .+10
BIS #100000,@FINUSE ;YES
CMP (DC)+,(DC)+
MOV #USRDM,-(SP) ;WE WILL SPROUT THIS JOB LATER SINCE THE KERNEL
MOV (DC)+,-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV (DC),-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV #SPRTIT,-(SP)
MOV (CC),DC ;RETURN THE DATA BLOCK TO FREE STORAGE LIST
MOV (DC),(CC)
MOV FRCPTR,(DC)
MOV DC,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
BR 7$
5$: MOV #3,(DC) ;RESET TEST COUNT
6$: MOV (CC),CC
7$: MOV (CC),DC ;CHECK NEXT JOB IN FORCE LIST
BNE CHKJOB
NOFJOB: TST (DAT)+ ;ON TO NEXT FORCE COMPONENT
CMP (BC)+,(BC)+
SOB EC,FRCLOP
BR SIGFRC
SPRTIT: FORK ;SPROUT FORCE JOB
SIGFRC: RTS PC ;THIS BRANCHES TO SPRTIT IF WE SAVED ANY JOBS TO
FRCEXT: ; SPROUT, OTHERWISE IT COMES HERE
;EXIT FROM LEVEL7 JOB ONE OF THREE WAYS:
CHKDIE: BIT #FINAL,FSTAT ;TIME TO DIE?
BNE KILFRC ;YES
RESFRC: RESCH7 #16. ;RE-SCHEDULE LEVEL 7 CODE
SUB #16.,(SP) ;SAVE DIFF. BETWEEN PREDICTED AND ACTUAL TIME
ADD (SP)+,FPTIME
BIT #FIRST,FSTAT ;1ST TIME THROUGH?
BNE 1$ ;YES
DEC BCKTME ;TIME TO SCHEDULE BACKGROUND JOB?
BGT FRCFIN ;NO
1$: BIT #BACKG,FSTAT ;BACKG JOB NOT YET THROUGH RUNNING?
BNE 2$ ;YES, BIG TROUBLE
MOV #FRATE,BCKTME ;RUN BACKGROUND JOB EVERY 3RD TIME
BIC #FIRST,FSTAT
FORK7 #FRCPD5,#FBACK,#USRDM ;START UP BCK JOB
2$: BIS #100000,@FINUSE ;SIGNAL CAT. ERROR
INC BOVLCK ;indicate overrun condition
FORK7 #BOVPDB,#BOVER,#USRDM ;SIGNAL BACKG. JOB NOT DONE
KILFRC: CLRB FSTAT ;CLEAR ALL RUN FLAGS
CLR CMPCPN
CLR FRCJTS
FRCFIN: DISMS7
;BACKGROUND JOB OVER-RUN ERROR MESSAGE JOB
BOVER:
.IFZ STDALN
ALERR BOVERM ;TELL OPERATOR, BACKGROUND JOB TOOK TOO LONG
.IFF
MOV #BOVERM,SG
JSR PC,TYPSTR
.ENDC
DEC BOVLCK ;OPEN LOCK
DISMISS
DATA
BOVPDB: PDBLK 6,30
BOVERM: .ASCIZ /BACKGROUND FORCE SYSTEM JOB TOOK TOO LONG TO EXECUTE.
YOU'RE IN BIG TROUBLE NOW, CALL FOR HELP.
/
.EVEN
CODE
;END OF FRCLV7
;FBACK - LEVEL 5 FORCE SENSING BACKGROUND JOB
;THE FOLLOWING JOB IS STARTED AFTER EVERY 3RD EXECUTION OF THE LEVEL 7
;FORCE SENSING JOB, "FRCLV7". IT IS USED FOR DOING ALL OF THE LENGTHY
;COMPUTATIONAL TASKS ASSOCIATED WITH THE FORCE SENSING/COMPLIANCE SYSTEM
;SUCH AS COMPUTING THE JACOBIAN, MAKING CORRECTIONS TO THE PLANNED
;TRAJECTORY AND SELECTING THE JOINTS TO BE FORCE SEROVED. THIS ROUTINE
;SHOULD ONLY BE EXECUTED BY DOING EITHER A "FORK", "FORK7", OR A "SCHEDU"
;THROUGH THE MONITOR.
FBACK: BIS #BACKG,FSTAT ;INDICATE BACKGROUND JOB RUNNING
TST FRCJTS ;ANY FORCE SERVOED JTS?
BNE 1$
MOV #THPTR,R1 ;NO, COMPUTE JACOBIAN USING CURRENT JT ANG
JMP CALJAC
1$: MOV SRVPTR,R1 ;PTRS TO SERVO DATA BLOCKS
MOVB FRCJTS,R0 ;JTS CURRENTLY FORCE SERVOING
MOV #FDTH,R2 ;SAVE DIFFERENCE IN HERE
BR 3$
2$: LDF POSERR(R5),AC0 ;CURRENT JOINT ANGLE - PREDICTED ANGLE
STF AC0,AC1 ;CONVERT TO RADIANS
MULF TOTQE(R5),AC1
MOV TH(R5),THN(R5) ;THN ← TH
MOV TH+2(R5),THN+2(R5)
STF AC1,(R2)+
ADDF DELTH(R5),AC0 ;CORRECT PREDICTED JT ANGLE
STF AC0,DELTH(R5)
3$: MOV (R1)+,R5 ;ON TO NEXT JT
ASR R0 ;FORCE SERVOED JT?
BCS 2$ ;YES
BNE 3$ ;NO
GETDEL: MOVB CMPCPN,R0 ;COMPUTE DIFFERENTIAL CHANGE TRANSFORM
MOV FJARAY,R1 ;JACOBIAN
MOV #DELPTR,R2 ;PTR TO DELTA ARRAY
MOV #6,R3
1$: CLRF AC0
ASR R0 ;COMPLIANT DIRECTION?
BCC 5$ ;NO
MOVB FRCJTS,R4 ;FORCE SERVOED JTS
MOV #FDTH,R5 ;CHANGE IN JT ANGLES
MOV R1,-(SP)
BR 4$
2$: LDF (R1),AC1 ;JACOBIAN ELEMENT*CHANGE IN JT ANGLE
MULF (R5)+,AC1
ADDF AC1,AC0 ;ACCUMULATED CHANGE
3$: ADD #6*4,R1 ;NEXT COLUMN OF JACOBIAN
4$: ASR R4 ;THIS JT FREE?
BCS 2$ ;YES
BNE 3$
MOV (SP)+,R1
5$: STF AC0,@(R2)+ ;PUT CHANGE INTO DIFF CHANGE TRANS
CMP (R1)+,(R1)+ ;NEXT ROW OF JACOBIAN
SOB R3,1$
NEGF AC0 ;-DEL THZ
STF AC0,DELTA+<3.*4>
LDF DELTA+<5.*4>,AC0;-DEL THX
NEGF AC0
STF AC0,DELTA+<7.*4>
LDF DELTA+<6.*4>,AC0;-DEL THY
NEGF AC0
STF AC0,DELTA+<2.*4>
MOV #TTRAN,R0 ;TTRAN ← DELTA * C_T_0(P) * 0_T_C(P+1)
MOV #DELTA,R1
MOV #CTC,R2
BIT #FTABLE,@FCARAY ;WORKING IN TABLE OR HAND SYSTEM?
BNE 6$ ;TABLE
JSR PC,MULSHR
MOV #OTCN,R0 ;0_T_C(CORRECTED P+1) ← 0_T_C(COR P)*TTRAN
MOV #OTCP,R1
MOV #TTRAN,R2
JSR PC,MATMUL
MOV #TTRAN,R0 ;0_T_6(CORRECTED P+1) ← 0_T_C(COR P+1)*C_T_6
MOV #OTCN,R1
MOV #CT6,R2
JSR PC,MULROT
BR 8$
6$: JSR PC,MULRSH ;TTRAN ← DELTA * C_R_0(P) * 0_R_0'(P-1)
MOV #OTON,R0 ;0_R_0'(P) ← 0_R_C(P) * TTRAN
MOV #OTCP,R1
MOV #TTRAN,R2
JSR PC,MULR
MOV #TTRAN,R0 ;0_R_6(CORRECTED P+1) ← 0_R_0'(P) * 0'_R_6(P+1)
MOV #OTON,R1
MOV #OT6N,R2
JSR PC,MULR
MOV #TTRAN+36.,R0 ;0_R_C(CORRECTED P) * P_DELTA
MOV #OTCP,R1
MOV #DELTA+36.,R2
MOV #1,R3
JSR PC,MULCOL
MOV #TTRAN+36.,R0 ;0_P_6(COR P+1)
MOV #OTCP+36.,R1
MOV #OT6N+36.,R2
MOV #3,R3
7$: LDF (R0),AC0
ADDF (R1)+,AC0
ADDF (R2)+,AC0
STF AC0,(R0)+
SOB R3,7$
8$: MOV #TTRAN,R0 ;COMPUTE CORRECTED JT ANGLES
MOV TTHCP,R1 ;PTR TO TABLE OF PTRS
MOV @FCARAY,R2 ;ARM BITS
JSR PC,SOLVE
TST R0 ;OUT OF RANGE?
BEQ SWTJTS
MOV @SRVPTR,DAT ;YES, STOP ALL ARM MOTION AND SIGNAL ERROR
BIS #FNOSOL,(DAT)
BIS #100000,@FINUSE
JMP FBCKDN
SWTJTS: MOV SRVPTR,R0 ;SWITCH TO NEW FORCE SERVOED JOINTS
SWAB FRCJTS
MOVB FRCJTS,R1 ;NEW JTS TO F.S.
MOV #FVALUE,R2 ;FORCE SERVO OUTPUT
MOV #TTH1,R3 ;PREDICTED POSITION
MOV #6,R4
.IFZ FRATE-3
LDF ONE48T,AC1 ;1/48
.IFF
LDF ONE32N,AC1 ;1/32
.ENDC
1$: MOV (R0)+,R5 ;SERVO DATA PTR
BIC #FSERVO,(R5)
ASR R1 ;FORCE SERVO THIS JT?
BCC 2$ ;NO
BIS #FSERVO,(R5) ;YES
LDF (R2)+,AC0 ;SET SERVO LEVEL
STF AC0,FLEVEL(R5)
2$: LDF TTHC1-TTH1(R3),AC0 ;COMPUTE CHANGE IN DELTH
SUBF (R3)+,AC0
SUBF DELTH(R5),AC0
MULF AC1,AC0
STF AC0,DELTHD(R5)
SOB R4,1$
; ["FBACK"]
;COMPUTE JACOBIAN, BEST SELECTIONS FOR NEXT FREE JOINTS AND THEIR FORCE LOADING
;COMPONENTS
MOV TTHCP,R1 ;COMPUTE JACOBIAN USING NEXT PREDICED JT ANGLES
CALJAC: MOV FCARAY,R0 ;R0 ← PTR TO FORCE TRANS, R2 ← ARM/TABLE BITS
MOV (R0)+,R2
MOV FJARAY,R3 ;PTR TO NEXT JACOBIAN ARRAY
MOV NEXTJ(R3),R3
JSR PC,JACOB
MOV #XFPTR,R0 ;COMPUTE SUM OF SQR'S FOR COMPONENTS WITH
MOV #6,R1 ; PENDING JOBS
MOV R3,R2
1$: TST (R0)+ ;ANY PENDING JOBS?
BEQ 3$ ;NO
CLRF AC0 ;YES, AC0 ← SUM OF SQUARES
MOV #6,R4
2$: LDF (R2),AC1
MULF AC1,AC1
ADD #24.,R2 ;NEXT NUMBER IN SAME ROW
ADDF AC1,AC0
SOB R4,2$
STF AC0,(R2) ;SAVE SUM OF SQUARES
SUB #<24.*6>-4,R2 ;ON TO NEXT ROW
BR 4$
3$: CLRF <24.*6>(R2) ;INDICATE NO SUM OF SQUARES
CMP (R2)+,(R2)+
4$: SOB R1,1$
MOV R3,FJARAY ;SWITCH FORCE SENSING SYSTEM TO NEW JACOBIAN
;DETERMINE JTS TO FORCE SERVO NEXT TIME AND THEIR FORCE OUTPUT VALUES
NXTFRS: MOVB #177,NEWJTS ;ASSUME NO FORCE SERVOED JTS
BISB NEWCPN,CMPCPN ;ADD NEW COMPLIANT DIRECTIONS TO OLD
BEQ CALFLV ;BRANCH IF NO COMPLIANCE TO DO
MOVB CMPCPN,NEWCPN
BR 6$
1$: CLR R4 ;SELECT BEST JOINT → JT WITH LARGEST
CLR R2 ; ELEMENT IN ROW OF JACOBIAN MATRIX*FACTOR
MOVB NEWJTS,R1
MOV #FSCALE,R0 ;SCALE FACTORS
2$: ASR R1 ;THIS JT ALREADY ASSIGNED TO FORCE SERVO?
BEQ 4$ ;END OF LIST
BCC 3$ ;YES
MOV (R3),R5 ;COMPARE ABS MSB VALUE TO CURRENT MAX
ADD (R0),R5 ;SCALE UP
BIC #100000,R5 ;ABS VALUE
CMP R5,R4
BLT 3$ ;LESS THAN MAX
MOV R5,R4 ;THIS IS THE NEW MAX
MOV #100,R2 ;JT BIT
3$: ASR R2
ADD #24.,R3 ;NEXT COLUMN OF JACOBIAN
TST (R0)+ ;NEXT SCALE FACTOR
BR 2$
4$: BICB R2,NEWJTS ;INDICATE JT SELECTED
SUB #24.*6,R3 ;RESET JACOBIAN PTR
5$: CMP (R3)+,(R3)+ ;NEXT ROW OF JACOBIAN
6$: ASRB NEWCPN ;COMPLIANT DIRECTION?
BCS 1$ ;YES
BNE 5$ ;CHECK NEXT COMPONENT
CALFLV: COMB NEWJTS ;NEXT FORCE SERVO JTS, 1 → FORCE SERVO
BICB #300,NEWJTS
BEQ NXTVAR ;NO FORCE SERVOED JTS
MOVB NEWJTS,R0 ;ARE SOME, COMPUTE FORCE LEVELS
MOV #FVALUE,R1 ;SAVE IN HERE
MOV FJARAY,R2 ;NEED JACOBIAN AGAIN
BR 6$
1$: MOV #CMPFRC,R3 ;DESIRED COMPLIANCE LEVELS
MOVB CMPCPN,R4 ; AND DIRECTIONS
MOV R2,R5
CLRF AC0 ;ACCUMULATED FORCE LEVEL
BR 4$
2$: LDF (R3)+,AC1 ;COMPLIANCE LEVEL*JACOBIAN
MULF (R5)+,AC1
ADDF AC1,AC0
BR 4$
3$: CMP (R3)+,(R3)+ ;UPDATE PTRS
CMP (R5)+,(R5)+
4$: ASR R4 ;COMPLIANT IN THIS DIRECTION?
BCS 2$ ;YES
BNE 3$ ;STILL MORE DIRECTIONS
STF AC0,(R1)+
5$: ADD #24.,R2 ;NEXT COLUMN OF JACOBIAN
6$: ASR R0 ;THIS JT FORCE SERVOED?
BCS 1$ ;YES
BNE 5$ ;NO
; ["FBACK"]
;COMPUTE THE VARIABLE THAT WE WILL NEED NEXT TIME: PREDICTED JT. ANGLES,
;PREDICTED TRANSFORM, PREDICTED CORRECTED TRANSFORM
NXTVAR: BIT #NXTFIN,FSTAT ;FINAL?
BNE 77$ ;YES
MOV FDATPT,R0 ;PTR TO START OF POLY. DATA LIST
MOV #TTH7,R1 ;COMPUTE AND SAVE HERE
MOV #6,R2
ADD #16.*FRATE,FPTIME;GET NEXT LEVEL 5 TIME
MOV FPTIME,R5
MOV FTTME,R4 ;PRESENT TOTAL SEGMENT TIME
BIT #PASSGO,FSTAT ;WAITING TO START?
BNE 1$ ;NO
ADD #16.*FRATE,R5 ;P+2 TIME PAST START WAIT?
SUB R4,R5
BGE 5$ ;YES, GO START FORCE SERVOING
BR .+6
77$: INC FSTAT ;INDICATE "FINAL" PART OF MOTION
CLR FRCJTS ;DONT PERMIT FORCE SERVOING
JMP FBCKDN
1$: CMP R5,R4 ;PAST END OF SEGMENT?
BLT 6$ ;NO
2$: MOV (R0),R3 ;YES,SWITCH TO A NEW SEGMENT
ADD R3,R0 ;MOVE PTR
TST (R0) ;FINAL SEGMENT?
BNE 4$ ;NO
MOV FPOLY,R0 ;FINISHED MOTION, GET FINAL SET POINTS
3$: LDF -(R0),AC0 ;A5
ADDF -(R0),AC0 ; + A4
ADDF -(R0),AC0 ; + A3
ADDF -(R0),AC0 ; + A2
ADDF -(R0),AC0 ; + A1
ADDF -(R0),AC0 ; + A0
STF AC0,-(R1)
SOB R2,3$
BIS #NXTFIN,FSTAT ;INDICATE P+2 IS END OF POLYS
CLRB NEWJTS ;DONT DO ANY MORE FORCE SERVOING AFTER P+1
BR COMTRN
4$: MOV R0,FDATPT ;NEW SEGMENT POINTER
ADD R3,FPOLY
SUB R4,R5 ;TIME INTO NEW SEGMENT
5$: MOV SEGTME(R0),R4 ;TOTAL TIME OF NEW SEG
LDCIF R4,AC0
CMP R5,R4 ;PAST END OF NEW SEGMENT?
BGE 2$ ;YES
MOV R4,FTTME ;NEW TOTAL SEGMENT TIME
MOV R5,FPTIME ;ADJUSTED PRESENT TIME
STF AC0,FFTIME
6$: LDCIF R5,AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
MOV FPOLY,R0 ;POLY ADDR
DIVF FFTIME,AC1 ;FRACTIONAL TIME INTO THIS SEGMENT
7$: LDF -(R0),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(R0),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(R0),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(R0),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(R0),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(R0),AC0 ; + A0
STF AC0,-(R1) ;→ TTH
SOB R2,7$
COMTRN: BIT #FTABLE,@FCARAY ;WORKING IN TABLE OR HAND SYSTEM?
BNE 2$ ;BRANCH IF TABLE
MOV #TTRAN,R0 ;GET C_T_0 AT TIME P+1
MOV #TCP2,R1
JSR PC,INVERT
MOV #CTC,R0 ;COMPUTE POLYNOMIAL TRANSFORMATION
MOV TTHP,R1
MOV @FCARAY,R2
JSR PC,UPDATE
MOV #TCP2,R0 ;TCP2 ← 0_T_C AT TIME P+2
MOV #CTC,R1 ;0_T_6
MOV FCARAY,R2 ;6_T_C
TST (R2)+
JSR PC,MULROT
MOV #CTC,R0 ;CTC ← C_T_0 (P+1)* 0_T_C(P+2)
MOV #TTRAN,R1
MOV #TCP2,R2
JSR PC,MULSHR
MOV #OTCN,R1 ;OTCP ← OTCN
MOV #OTCP,R2
MOV #12.,R3
1$: MOV (R1)+,(R2)+
MOV (R1)+,(R2)+
SOB R3,1$
BR ISGO
2$: MOV #OTCP,R0 ;GET 0_T_C AT TIME P+1
MOV FCARAY,R1 ;0_R_C SPECIFIED BY USER
TST (R1)+
MOV #TTRAN,R2 ;AND SAVE TRANSPOSE IN HERE
MOV #TRANSP,R3
MOV #9.,R4
3$: LDF (R1)+,AC0
STF AC0,(R2)
ADD (R3)+,R2
STF AC0,(R0)+
SOB R4,3$
LDF (R2)+,AC0 ;0_P_C(P+1) ← 0_P_6(COR P+1)
STF AC0,(R0)+
LDF (R2)+,AC0
STF AC0,(R0)+
LDF (R2),AC0
STF AC0,(R0)
MOV #CTC,R0 ;C_R_6(P+1) * 0_R_0'(P)
MOV #TTRAN,R1
MOV #OTON,R2
JSR PC,MULSHR
MOV #OT6N,R0 ;COMPUTE POLYNOMIAL TRANSFORMATION
MOV TTHP,R1
MOV @FCARAY,R2
JSR PC,UPDATE
MOV #OP6,R0 ;OP6 ← NEXT POLY POSITION
MOV #OT6N+36.,R1 ;O_P_6(P+1) ← CHANGE IN POSITION
MOV #3,R2
4$: LDF (R1),AC0
SUBF (R0),AC0
MOV (R1),(R0)+
MOV 2(R1),(R0)+
STF AC0,(R1)+
SOB R2,4$
;EXIT CLEANLY
ISGO: BIS #PASSGO,FSTAT ;YES, WE CAN START COMPUTING
FBCKDN:
.IFNZ FRCDAT
MOV FDATA,R0
MOV FSTAT,(R0)+
MOV CMPCPN,(R0)+
MOV FRCJTS,(R0)+
MOV R0,FDATA
.ENDC
BIC #BACKG,FSTAT ;BACKGROUND JOB DONE
DISMIS
.IFF ;THIS MATCHES THE ".IFZ DIAGY" AT THE START OF THE FORCE RTNS.
FRCLV7:
.ENDC
;END OF "FBACK"
;DEFINITIONS FOR PROGRAMS USING WRIST SENSOR
;BITS INDICATING WHICH FORCES ARE TO BE GATHERED ARE DEFINED AS FOLLOWS:
XFBIT == 1
YFBIT == 2
ZFBIT == 4
XMBIT == 10
YMBIT == 20
ZMBIT == 40
TBLBIT == 200 ;CONVERT READINGS TO TABLE COORDS (DEFAULT IS HAND COORDS)
;BITS INDICATING WHICH JOINTS TO GATHER RESOLVED TORQUES FOR ARE:
T1BIT == 400 ;JOINT 1
T2BIT == 1000 ;2
T3BIT == 2000 ;3
T4BIT == 4000 ;4
T5BIT == 10000 ;5
T6BIT == 20000 ;6
;SETBAS - PROCEDURE TO SET THE BASE READINGS FOR THE FORCE WRIST
;THIS PROCEDURE IS USED TO TAKE AN INITIAL SET OF READINGS TO BE
;USED AS AN OFFSET IN RESOLVING THE SCHEINMAN FORCE SENSING WRIST.
;THE ONLY ARGUMENT REQUIRED BY THIS ROUTINE IS A POINTER
;TO A INTEGER ARRAY 8 WORDS LONG. AFTER EXECUTION, THE STRAIN
;GAGE READINGS ARE RETURNED IN THIS ARRAY AS WELL AS BEING LEFT
;IN AN INTERNAL ARRAY WHICH IS USED BY THE FORCE RESOLVING
;ROUTINE "WRIST". IF YOU DON'T NEED THE STRAIN GAGE READINGS,
;THE REGISTER WHICH NORMALLY CONTAINS THE ARRAY POINTER SHOULD BE
;SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #SAVEBASE,R0 ;SET TO 0 IF NOT NEEDED
; JSR PC,SETBAS
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0 PASSES AN ARGUMENT AND IS NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
SETBAS: INC BASLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC BASLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR SETBAS
1$: MOV R0,BASPTR ;SET POINTER TO RETURN BASE VALUES
BNE .+10 ;DONT RETURN VALUES?
MOV #BASRDG,BASPTR
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),BASEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #BASPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
DEC BASLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 7 CODE FOR "SETBAS"
BASLV7: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOV DAT,-(SP) ;SAVE POINTER TO BASRDG
MOV #8.,DC
2$: CLR (DAT)+ ;CLEAR BASRDG FOR ACUMULATING READINGS
SOB DC,2$
MOV #4,DC
3$: MOV #30,BC ;GET FIRST WRIST CHANNEL ADDRESS
MOVB BC,ADCCHN ;START FIRST CONVERSION
MOV #10,CC ;SET COUNTER TO READ 8. CHANNELS
MOV (SP),DAT ;GET FIRST BASRDG ADDRESS
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL .-4
ADD ADCVAL,(DAT)+ ;SAVE NEW READING
TSTB (BC)+ ;POINT TO NEXT CHANNEL
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
SOB CC,1$ ;REPEAT UNTIL DONE 8 SENSORS
SOB DC,3$ ;REPEAT READINGS 4 TIMES
MOV BASPTR,DC ;GET PTR TO USER STORAGE ARRAY
MOV (SP)+,DAT
MOV #8.,AC
4$: ASR (DAT) ;DIVIDE BY 4
ASR (DAT)
MOV (DAT)+,(DC)+ ;SAVE DATA FOR USER
SOB AC,4$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
5$: EVSIG BASEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
;LOCAL STORAGE AREA
DATA
BASEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
BASPTR: .BLKW 1 ;PTR TO ARRAY TO STORE READINGS IN FOR USER
BASRDG: .BLKW 8. ;ARRAY TO STORE READINGS FOR "WRIST"
BASPPB: BASPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0
BASPDB: PDBLK7 BASLV7,BASRDG,20
BASLCK: -1 ;-1 → SETBAS READY FOR USE, 0 → LOCKED UP
CODE
;END OF "SETBAS"
;WRIST - PROCEDURE TO READ AND RESOLVE THE FORCE WRIST READINGS
;THIS PROCEDURE IS USED TO READ AND RESOLVE THE FORCE WRIST STRAIN
;GAGE READINGS INTO EQUIVALENT FORCE AND MOMENT COMPONENTS. THE
;ARGUMENTS REQUIRED BY THIS ROUTINE ARE A POINTER TO A 6x8 CALIBRATION
;TABLE USED TO CONVERT THE READINGS TO FORCES/MOMENTS AND A POINTER
;TO A 1x6 ARRAY INTO WHICH THE COMPUTE FORCE/MOMENT COMPONENTS ARE STORED.
;IF THE STANDARD CALIBRATION MATRIX TO RESOLVE FORCES AND MOMENTS
;IN THE REFERENCE FRAME OF THE FORCE WRIST IS DESIRED, THE POINTER
;TO THE 6x8 CAN BE SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #CALTAB,R0 ;SET TO 0 IF NOT NEEDED
; MOV #FORCES,R1 ;PTR TO TABLE OF RESULTS
; JSR PC,WRIST
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
WRIST: INC WSTLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC WSTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR WRIST
1$: MOV R0,CALPTR ;SET POINTER TO CALIBRATION DATA
BNE .+10
MOV #STDCAL,CALPTR ;USE STANDARD CALIBRATION AS DEFAULT
MOV R1,FORPTR ;STORE COMPUTED F/M'S IN HERE
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),WSTEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #WSTPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
DEC WSTLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 7 CODE FOR "WRIST"
WSTLV7: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOVB #30,ADCCHN ;START ADC CONVERTING FIRST CHANNEL
MOV FORPTR,BC ;INITIALIZE ARRAY TO SAVE DATA IN
MOV #6,AC ;ZERO ALL 6 ELEMENTS
CLRF (BC)+ ;*K COULD PUT XFORMED BASE READINGS HERE
SOB AC,.-2
MOV #31,BC ;POINT TO NEXT WRIST CHANNEL
MOV #10,WSTCNT ;SET COUNTER TO READ 8. CHANNELS
MOV #BASRDG,CC ;POINTER TO BASE SENSOR READINGS
MOV (DAT),DAT ;POINT TO CALIBRATION TABLE
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL 1$
MOV ADCVAL,AC
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
TSTB (BC)+ ;POINT TO NEXT WRIST CHANNEL
SUB (CC)+,AC ;SUBTRACT BASE READING FROM SENSOR READING
LDCIF AC,AC1 ;SAVE CORRECTED READING
MOV FORPTR,EC ;SAVE SUMS OF PRODUCTS IN HERE
MOV #6,DC ;SIX SUMS IN ALL, ONE FOR EACH FORCE/MOMENT
2$: LDF (DAT)+,AC0 ;MULT ADC READING TIMES CALIBRATION MATRIX
MULF AC1,AC0
ADDF (EC),AC0 ;ACCUMULATE FORCE COMPONENTS
STF AC0,(EC)+
SOB DC,2$ ;REPEAT FOR ALL SIX FORCES/MOMENTS
DEC WSTCNT ;REPEAT FOR ALL EIGHT STRAIN GAGES
BGT 1$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
;RTS PC
EVSIG WSTEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
;LOCAL STORAGE AREA
DATA
CALPTR: .BLKW 1 ;PTR TO CALIBRATION TABLE
FORPTR: .BLKW 1 ;PTR TO ARRAY TO SAVE RESULTS IN
WSTCNT: .BLKW 1 ;LOOP COUNTER
WSTLCK: -1 ;-1 → WRIST READY FOR USE, 0 → LOCKED UP
WSTEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
;PUT CALIBRATION DATA AT FRONT OF ARM DATA IN AL VERSION OF CODE
.IFZ STDALN
YXX == .
. = ARMDAT
.ENDC
STDCAL: ;STANDARD CALIBRATION MATRIX TRANSPOSE
.FLT2 .3594420E-2 , -.8563111E-1 , .1285777E1 , -.3646213E1 , -.2937869 , -.2476264
.FLT2 .1874845E1 , -.1949082E-1 , .3409287E-1 , .2371486E1 , -.4910622 , .2427377E1
.FLT2 -.1786587 , -.1268213E-1 , .1946006E1 , .8202766 , -.3587131E1 , -.4727840
.FLT2 .2462366 , .2037721E1 , -.1294795 , -.1798717 , .3207096 , .3358268E1
.FLT2 -.1856604 , .4546825E-1 , -.1484695E1 , -.4280565E1 , .2758572 , .3213347
.FLT2 .1924275E1 , -.2987852 , .1159742 , .2039192E1 , -.4782244 , -.3027550E1
.FLT2 .5855758E-1 , -.1566935 , -.1627386E1 , .8823713E-1 , -.3473955E1 , .3031765
.FLT2 -.1508125 , .2152276E1 , .7111265E-1 , -.9404183 , .3356924 , -.2835498E1
.IFZ STDALN
. = YXX
.ENDC
WSTPPB: WSTPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0
WSTPDB: PDBLK7 WSTLV7,CALPTR,20
CODE
;END OF "WRIST"
;GATHER - PROCEDURE TO COLLECT FORCE DATA
CODE
;THIS PROCEDURE INITALIZES THE DATA GATHERING SYSTEM. THE FORCE WRIST IS
;READ AT 30 Hz AND SELECTED FORCES ARE STORED IN AN ARRAY. BITS IN ANY
;MEANINGFUL COMBINATION IN R0 DETERMINE WHICH FORCES ARE TO BE RECORDED
;ACCORDING TO THE DEFINITIONS ON PAGE 53 ( ONLY HAND COORDS ARE VALID NOW ).
;R1 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR FLOATING POINT
;STORAGE AND R2 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR INTEGER
;STORAGE. IF R1 OR R2 IS ZERO DEFAULT AREAS WILL BE USED. R3 CONTAINS
;AN IDENTIFICATION NUMBER TO BE PASSED BACK AT BEGINNING OF INTEGER BUF.
;
;THIS ROUTINE SHOULD BE CALLED JUST PRIOR TO CALLING THE MOVE FOR WHICH
;DATA IS TO BE COLLECTED:
;
; MOV #FBITS,R0
; MOV #FPPTR,R1
; MOV #IPTR ,R2
; MOV #ID,R3
; JSR PC,GATHER
;
;R0,R1 AND R2 PASS ARGUMENTS AND ARE CHHANGED
GATHER:
TST R1 ;IF R1|R2 =0 USE DEFAULT AREAS
BEQ 1$
TST R2
BNE 2$
1$: MOV #FPPTR,R1 ;POINT TO FP POINTER
MOV #GAREA,FPBUF
MOV #GAREA,FPPTR
MOV #INTPTR,R2 ;POINT TO INT POINTER
MOV #GIAREA,INTBUF
MOV #GIAREA,INTPTR
2$: TSTB R0 ;SHOULD IT BE IN HAND OR TABLE COORDS?
BEQ 5$ ;QUIT IF NO CONTROL BITS
BMI 5$ ;FOR NOW JUST IN HAND COORDS
MOV R0,GDATA ;SAVE CONTROL BITS
MOV R1,FPSAV ;SAVE POINTER TO FP POINTER
MOV R2,ISAV ;SAVE POINTER TO INTEGER POINTER
MOV (R2),R0 ;GET INTEGER PTR *K
MOV R3,(R0)+ ;SAVE ID #
MOV R0,(R2) ;REPLACE UPDATED PTR
CLR GTIME ;ZERO TIMES COUNTER
CLR R0 ;ZERO WRIST READINGS
JSR PC,SETBASE
5$: RTS PC
;LEVEL 7 JOB TO GATHER DATA
;AT START DAT←#EPSF
GATLV7: TST GDATA,R0 ;CHECK CONTROL WORD
BEQ GATFIN ;QUIT IF CLEARED
RESCH7 #16. ;ELSE RESCHEDULE JOB
TST (SP)+ ;POP SCHEDULE TIME OFF STACK
INC GTIME
BIT #1,GTIME ;TAKE READINGS EVERY 2ND TIME
BEQ RNS ;
GETEPS: MOVB #30,ADCCHN ;START FIRST CONVERSION
MOV #BASRDG,AC ;POINT TO BASE READINGS
MOV #8.,BC ;EIGHT SENSORS TO READ
MOV #31,DC ;NEXT CHANNEL TO READ
MOV #EPS,EC ;INTEGER READING ARRAY
1$: TSTB ADCSR
BPL 1$
MOV ADCVAL,CC ;GET READING
MOVB DC,ADCCHN ;START NEXT READING
TSTB (DC)+ ;POINT TO NEXT CHANNEL
SUB (AC)+,CC ;SUBTRACT BASE READING
;FILTERING ON CC GOES HERE
MOV CC,(EC)+ ;ADD CHANGE TO EPS ARRAY
LDCIF CC,AC0
STF AC0,(DAT)+ ;STORE FLOATING POINT EPS
DEC BC
BGT 1$
BR GATFIN ;DONE WITH READINGS
;RESOLVE AND STORE READINGS AS NEEDED
RNS: MOV GPTR,R1 ;GET DATA POINTER
MOV GDATA,R0 ;GET CONTROL BITS
BIC #100000,R0 ;CLEAR SIGN BIT
MOV #STDCAL,R2 ;POINT TO CALIBRATION MATRIX
GCK: BIT #1,R0 ;STORE THIS READING?
BNE 2$
ADD #4,R2 ;NO, ADVANCE CAL POINTER TO NEXT COL
BR 3$
2$: CLRF AC1 ;COMPUT THIS FORCE COMPONENT
MOV #EPSF,R4 ;POINT TO GAGE READINGS
MOV #8.,R3 ;8 *'S
1$: LDF (R2),AC0 ;GET CAL ENTRY
MULF (R4)+,AC0 ;* EPSF
ADD #6*4,R2 ;POINT TO NEXT ITEM IN COL.
ADDF AC0,AC1 ;ACCUMULATE
DEC R3
BGT 1$
STF AC1,(R1)+ ;STORE FORCE READING IN DATA AREA
ADD #4-<8.*6*4>,R2 ;POINT TO NEXT COL
3$: ASR R0 ;CHECK NEXT CONTROL BIT
BNE GCK ;CONTINUE UNTIL NO MORE BTS
MOV R1,GPTR ;SAVE DATA POINTER
GATFIN: DISMS7 ;KILL GATLV7
;GATHER DATA AREA
DATA
GATPPB: GATPBB ;ARRAY TO INITALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
GATPBB: PDBLK7 GATLV7,EPSF,20
GDATA: .WORD 0 ;CONTAINS BITS INDICATING COMPONENTS TO GATHER
GTIME: .WORD 0 ;INDICATES WHICH PASS WE ARE ON
GPTR: .WORD 0 ;POINTS TO NEXT FP STORAGE LOCATION removed by MSM
FPSAV: .WORD 0
GIPTR: .WORD 0 ;POINTS TO INTEGER STORAGE LOCATION
ISAV: .WORD 0
EPS: .BLKW 8. ;STORAGE FOR STRAIN GAGE READINGS
EPSF: .BLKW 2*8. ;FP STORAGE FOR READINGS
;END OF GATHER
;FORCE SENSING ROUTINES DATA AREA
DATA
FZAPS: ;START OF AREA ZEROED BY "INTARM"
FSTAT: .WORD 0 ;FORCE SENSING SYSTEM STATUS WORD, CONTAINS:
;NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
;FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;RUN ==4 ;LEVEL 7 CODE SCHEDULED TO EXECUTE
PASSGO ==10 ;LEVEL 5 CODE INITIALIZED AND READY TO GO
BACKG ==20 ;BACKGROUND JOB RUNNING AT LEVEL 5
;FIRST ==40 ;FIRST PASS THRU FORCE CODE
; 177400 ;≠0 MEANS LEVEL 7 CODE LOCKED OUT
CMPCPN: .BYTE 0 ;COMPLIANT COMPONENTS 1→FX, 40→MZ
NEWCPN: .BYTE 0 ;NEW COMPONENTS
FRCJTS: .BYTE 0 ;FORCE SERVOED ARM JTS 1→JT 1, 40→JT 6
NEWJTS: .BYTE 0 ;NEW FORCE SERVOED JTS
FRCNUM: .WORD 0 ;NUMBER OF JOBS PENDING
BCKTME: .WORD 3 ;=0 WHEN TIME TO RUN BACKGROUND JOB
XFPTR: .WORD 0 ;LINK TO JOB BLKS
YFPTR: .WORD 0
ZFPTR: .WORD 0
XMPTR: .WORD 0
YMPTR: .WORD 0
ZMPTR: .WORD 0
FZAPE: ;END OF ZERO AREA
FCARAY: C1 ;PTR TO CURRENT C ARRAY AND ARM DATA
C1: 0 ;YELARM/BLUARM AND TABLE/HAND BITS
.BLKW 12.*2 ;FORCE TRANS
C2
C2: 0
.BLKW 12.*2
C1
NEXTC== C2-C1-2 ;REL. PTR TO RING LINKAGE WORD
FJARAY: J1 ;PTR TO CURRENT JACOBIAN ARRAY
J1: .BLKW 42.*2 ;JACOBIAN ARRAY + SUM OF SQUARES
J2
J2: .BLKW 42.*2
J1
NEXTJ== J2-J1-2 ;REL. PTR TO RING LINKAGE WORD
DELTA: .FLT2 1.0,0.0,0.0 ;DIFFERENTIAL CHANGE TRANSFORMATION
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
DELPTR: DELTA+<9.*4> ;DEL X
DELTA+<10.*4> ;DEL Y
DELTA+<11.*4> ;DEL Z
DELTA+<5.*4> ;DEL THX
DELTA+<6.*4> ;DEL THY
DELTA+<1.*4> ;DEL THZ
IMAT: .FLT2 1.0,0.0,0.0 ;IDENTITY MATRIX
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
TTRAN: .BLKW 12.*2 ;TEMPORARY TRANSFORM STORAGE
CTC: .BLKW 12.*2 ;C_T_0 (P+1)* 0_T_C(P+2)
OT6N:
TCP2: .BLKW 12.*2 ;0_T_C POLY AT TIME P+2
OTCP: .BLKW 12.*2 ;0_T_C CORRECTED FOR TIME P+1
OTON:
OTCN: .BLKW 12.*2 ;0_T_C CORRECTED FOR TIME P+2
OP6: .BLKW 3*2 ;OLD POLYNOMIAL POSITION
CMPFRC: .BLKW 6*2 ;FORCE COMPLIANCE LEVELS
FVALUE: .BLKW 6*2 ;JOINT FORCE COMPLIANCE LEVELS
TCOR: .BLKW 12.*2 ;CORRECT PREDICTED TRANSFORMATION
TTHP: TTH-14.
TTH: .WORD .+14,.+16,.+20 ;TABLE OF PTRS TO STORAGE FOR JOINT ANGLES
.WORD .+22,.+24,.+26
TTH1: .BLKW 6*2
TTH7:
TTHCP: TTHC-14.
TTHC: .WORD .+14,.+16,.+20 ;TABLE OF PTRS TO STORAGE FOR JOINT ANGLES
.WORD .+22,.+24,.+26
TTHC1: .BLKW 6*2
TRANSP: .WORD 12.,12.,-20.,12.,12.,-20.,12.,12.,4
FSCALE: 0 ;JACOBIAN SCALE FACTORS , *1
0 ;*1
1200 ;*32
600 ;*8
600 ;*8
1000 ;*16
FDATPT: .BLKW 1 ;POLYNOMIAL DATA INFORMATION, DATPT PTR
FPOLY: .BLKW 1 ;POLY PTR OF JT 6 + 4
FINUSE: .BLKW 1 ;INUSE
FPTIME: .BLKW 1 ;PTIME
FTTME: .BLKW 1 ;TTIME
FFTIME: .BLKW 2 ;FTTIME
FDTH: .BLKW 6*2 ;CHANGE IN FORCE SERVOED JOINTS ANGLES
SRVPTR: .BLKW 1 ;PTRS TO ARM JTS
FRCPTR: FRCJOB ;PTR TO CHAIN OF FREE BLKS IN JOB LIST
FRCJOB: .BLKW 7*FRCBLK;JOB BLOCKS CONTAIN:
; WRD 1: LINK TO NEXT BLOCK
; WRD 2,3: F.P. MAGNITUDE
; WRD 4: FORCE OPTION BITS
; WRD 5: COUNTER FOR # OF TIMES TEST OK
; WRD 6,7: STRT ADDR OF JOB&PDB
FRCPPB: FRCPBB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
FRCPBB: PDBLK7 FRCLV7,XFPTR,20
FRCPD5: PDBLK 5,100,FP;LEVEL 5 PDB
;END OF "FORCE" ROUTINES
;VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
VELERR: .FLT2 0.0 ;VELOCITY ERROR, THETA DOT - THETA DOT PREDICTED
TORQUE: .BLKW 2 ;OUTPUT MOTOR TORQUE
A05TH: .FLT2 0.0 ;5TH ORDER CORRECTION EQUATION
A15TH: .FLT2 0.0
A25TH: .FLT2 0.0
A35TH: .FLT2 10.0
A45TH: .FLT2 -15.0
A55TH: .FLT2 6.0
FP6: .FLT2 6.0
FP10: .FLT2 10.0
FP15: .FLT2 15.0
FP30: .FLT2 30.0
FP60: .FLT2 60.0
FP90: .FLT2 90.0
ONE48T: .FLT2 .020833333
ONE32N: .FLT2 .03125
;SINUSOIDAL TABLE FOR JOINT WOBBLING
WOBTAB: .FLT2 .0000000, .3826834, .7071068, .9238795
.FLT2 1.000000, .9238795, .7071068, .3826834
.FLT2 .0000000,-.3826834,-.7071068,-.9238795
.FLT2 -1.000000,-.9238795,-.7071068,-.3826834
YWOBMG: .FLT2 0.0 ;MAGNITUDE OF YELLOW ARM WOBBLE
BWOBMG: .FLT2 0.0 ; " " BLUE " "
SRV01: NONEX ;YELLOW ARM JOINT #1 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 1 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140511,7733 ;TH -Pi
.WORD 140511,7733 ;THP -Pi
.WORD 140511,7733 ;THN -Pi
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
0 ;POTCHN see armdoctxt[armted] *k
8. ;TACCHN ditto
174000 ;DACADD
.BYTE 1 ;DACCHN
.BYTE 0 ;DACVAL
401 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV01,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV02: NONEX ;YELLOW ARM JOINT #2 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 2 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
1 ;POTCHN
9. ;TACCHN
174000 ;DACADD
.BYTE 2 ;DACCHN
.BYTE 0 ;DACVAL
1002 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV02,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV03: NONEX ;YELLOW ARM JOINT #3 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 3 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 41100,0 ;TH 3.0 inches
.WORD 41100,0 ;THP 3.0 inches
.WORD 41100,0 ;THN 3.0 inches
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
2 ;POTCHN
10. ;TACCHN
174000 ;DACADD
.BYTE 3 ;DACCHN
.BYTE 0 ;DACVAL
2004 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS multiple wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV03,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV04: NONEX ;YELLOW ARM JOINT #4 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 4 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCII
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
3 ;POTCHN
-1. ;TACCHN
174000 ;DACADD
.BYTE 4 ;DACCHN
.BYTE 0 ;DACVAL
4010 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV04,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV05: NONEX ;YELLOW ARM JOINT #5 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 5 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 40311,7733 ;TH Pi/2
.WORD 40311,7733 ;THP Pi/2
.WORD 40311,7733 ;THN Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 5 ;DACCHN
.BYTE 0 ;DACVAL
10020 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV05,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV06: NONEX ;YELLOW ARM JOINT #6 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 6 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 0,0 ;TH 0 degrees
.WORD 0,0 ;THP 0 degrees
.WORD 0,0 ;THN 0 degrees
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
5 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 6 ;DACCHN
.BYTE 0 ;DACVAL
20040 ;DACINF
.WORD 0,0 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV06,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV07: NONEX ;YELLOW ARM HAND SERVO DATA BLOCK
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 7 ;SRVNUM
.BYTE YELHND ;MECHSM
.WORD 40500,0 ;TH 3.0 inches
.WORD 40500,0 ;THP 3.0 inches
.WORD 40500,0 ;THN 3.0 inches
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.WORD 0,0 ;V0
.WORD 0,0 ;CI
.WORD 0,0 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.WORD 0,0 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
7 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 7 ;DACCHN
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.WORD 0,0 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 0 ;DITHER
.WORD 0,0 ;SCALE
.WORD 0,0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV07,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV08: 0 ;BLUE ARM JOINT #1 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 8. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 180.0 ;TH
.FLT2 180.0 ;THP
.FLT2 180.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 .10 ;ERRTOL
.FLT2 -.0320 ;KV -.05
.FLT2 -125000.0 ;KE -150000.0
.FLT2 -5.854@11 ;KI -.6@12
.FLT2 222.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .01 ;CKI
.FLT2 -2000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .1 ;CKF
1000. ;POT
0 ;TACH
21 ;POTCHN
20 ;TACCHN
174000 ;DACADD
.BYTE 1 ;DACCHN
.BYTE 0 ;DACVAL
401 ;DACINF
.FLT2 .146092@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.08653846 ;SCALE
.FLT2 259.5289 ;OFFSET
.FLT2 190.00 ;USTOP
.FLT2 -45.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.6570@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV08,20 ;PROCESS DESCRIPTOR BLOCK
SRV09: 0 ;BLUE ARM JOINT #2 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 9. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.10 ;ERRTOL
.FLT2 -.0333 ;KV -.06
.FLT2 -1.25@5 ;KE -1.25@5
.FLT2 -9.0@11 ;KI -9.0@11
.FLT2 400.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.44@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;CKI
.FLT2 -2000.0 ;CKV
.FLT2 -90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .1 ;CKF
0 ;POT
0 ;TACH
14 ;POTCHN
10 ;TACCHN
174000 ;DACADD
.BYTE 2 ;DACCHN
.BYTE 0 ;DACVAL
1002 ;DACINF
.FLT2 .5353@-2 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .06686479 ;SCALE
.FLT2 -222.9941 ;OFFSET
.FLT2 -50.00 ;USTOP
.FLT2 -165.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.5401@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV09,20 ;PROCESS DESCRIPTOR BLOCK
SRV10: 0 ;BLUE ARM JOINT #3 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 10. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 12.0 ;TH inches
.FLT2 12.0 ;THP inches
.FLT2 12.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.02 ;ERRTOL
.FLT2 -0.0537 ;KV -0.03
.FLT2 -500.0 ;KE -200.0
.FLT2 -1.86@7 ;KI -4.0@6
.FLT2 5.0 ;V0 12.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 484940.0 ;CII
.BLKW 2 ;DCII
.FLT2 484940.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 0.0 ;CKI
.FLT2 -2000.0 ;CKV
.FLT2 14.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .01 ;CKF
.BLKW 1 ;POT
.BLKW 1 ;TACH
15 ;POTCHN
11 ;TACCHN
174000 ;DACADD
.BYTE 3 ;DACCHN
.BYTE 0 ;DACVAL
2004 ;DACINF
.FLT2 -0.43478 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .9099181@-2 ;SCALE
.FLT2 .8012 ;OFFSET
.FLT2 33.00 ;USTOP
.FLT2 6.75 ;LSTOP
.FLT2 1.0 ;STPBND
.FLT2 3.0457@-5 ;VSCALE
177 ;MAXDRV
1 ;TACHZ0
0 ;WIPERS single
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV10,20 ;PROCESS DESCRIPTOR BLOCK
SRV11: 0 ;BLUE ARM JOINT #4 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 11. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.2 ;ERRTOL
.FLT2 -.03 ;KV was -0.0448
.FLT2 -15000.0 ;KE -7000.0
.FLT2 -3.08@9 ;KI -3.0@9
.FLT2 19.0 ;V0 70.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@4 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .05 ;CKI
.FLT2 -2000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .01 ;CKF
0 ;POT
0 ;TACH
2 ;POTCHN
6 ;TACCHN
174000 ;DACADD
.BYTE 4 ;DACCHN
.BYTE 0 ;DACVAL
4010 ;DACINF
.FLT2 .9924@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .0925687 ;SCALE
.FLT2 -157.569406 ;OFFSET
.FLT2 205.00 ;USTOP
.FLT2 -295.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -4.0894@-4 ;VSCALE
177 ;MAXDRV
51. ;TACHZ0
MWIP11 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV11,20 ;PROCESS DESCRIPTOR BLOCK
.WORD 0,0,0,0,0 ;MULT. WIPER DATA
2 ;A/D CHAN WIPER #1
.FLT2 -518.07277 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 -322.35 ;OFFSET WAS -327.35677
.FLT2 .0870195 ;SCALE
MWIP11: 2 ;A/D CHAN WIPER #1
.FLT2 -157.569406 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 38.18 ;OFFSET WAS 32.186366
.FLT2 .0870195 ;SCALE
0
SRV12: 0 ;BLUE ARM JOINT #5 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 12. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 90.0 ;TH
.FLT2 90.0 ;THP
.FLT2 90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.0242 ;KV -0.04
.FLT2 -15000.0 ;KE -2000.0
.FLT2 -1.34@9 ;KI -1.0@9
.FLT2 15.0 ;V0 60.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .1 ;CKI
.FLT2 -100.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .02 ;CKF
0 ;POT
0 ;TACH
3 ;POTCHN
7 ;TACCHN
174000 ;DACADD
.BYTE 5 ;DACCHN
.BYTE 0 ;DACVAL
10020 ;DACINF
.FLT2 -.1299540 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .6711409@-1 ;SCALE
.FLT2 -136.9799 ;OFFSET
.FLT2 95.00 ;USTOP
.FLT2 -95.00 ;LSTOP
.FLT2 5.0 ;STPBND
.FLT2 -3.7919@-4 ;VSCALE
177 ;MAXDRV
23. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV12,20 ;PROCESS DESCRIPTOR BLOCK
SRV13: 0 ;BLUE ARM JOINT #6 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 13. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 0.0 ;TH
.FLT2 0.0 ;THP
.FLT2 0.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.7 ;ERRTOL
.FLT2 -0.1375 ;KV -0.1
.FLT2 -800.0 ;KE -1000.0
.FLT2 -2.18@8 ;KI -5.0@7
.FLT2 9.0 ;V0 25.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .05 ;CKI
.FLT2 -200.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .01 ;CKF
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 6 ;DACCHN
.BYTE 0 ;DACVAL
20040 ;DACINF
.FLT2 0.075188 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
5. ;DITHER
.FLT2 -.850260@-1 ;SCALE
.FLT2 223.023 ;OFFSET
.FLT2 200.00 ;USTOP
.FLT2 -110.00 ;LSTOP
.FLT2 5.0 ;STPBND
.BLKW 2 ;VSCALE no tachometer
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV13,20 ;PROCESS DESCRIPTOR BLOCK
SRV14: 0 ;BLUE ARM HAND SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.BYTE 14. ;SRVNUM
.BYTE BLUHND ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
5 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 7 ;DACCHN
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -1.00 ;LSTOP was -.2
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV14,20 ;PROCESS DESCRIPTOR BLOCK
SRV15: 0 ;VISE SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.BYTE 15. ;SRVNUM
.BYTE VISE ;MECHSM *K
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 1.0@8 ;ERRTOL *k large error tolerence
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;CKI
.FLT2 -1000.0 ;CKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;CK
.FLT2 .05 ;CKF
0 ;POT
0 ;TACH
5 ;POTCHN READ HAND FOR NOW *K
-1 ;TACCHN none
174000 ;DACADD
.BYTE 10 ;DACCHN *K
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -0.20 ;LSTOP
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV15,20 ;PROCESS DESCRIPTOR BLOCK *k
;DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
;THE DIAGNOSTIC DATA ARRAY IS OF THE FOLLOWING FORM
;
; DBUF: LAST WORD
; +2 : PTIME ;TIME SINCE THE START OF CURRENT SEGMENT(MSEC)
; +4 : THETA ;JOINT ANGLE FROM A/D READING(F.P.)
; +10 : THD ;JOINT VELOCITY FROM A/D (F.P.)
; +14 : ERRPOS ;POSITION ERROR (F.P.)
; +20 : VELERR ;VELOCITY ERROR (F.P.)
; +24 : DACOUT ;OUTPUT MOTOR DAC
;
;THE SEQUENCE REPEATS STARTING WITH SRVNUM AGAIN. THE POINTER "DBUF" IS
;ALWAYS LEFT POINTING AT THE FIRST UNUSED WORD IN THE DIAGNOSTIC BUFFER.
DBUF:
.IFNZ DIAGY
.+2 ;STORAGE AREA FOR DIAGNOSTIC JOINT DATA
.BLKW DBUFL
ACTUAL: 0 ;SET 0 = SEND ERROR TERMS, 1 = ACTUAL ADC READINGS
.ENDC
.IFNZ TACCAL
BELL: .BYTE 7,0
.ENDC
.IFNZ TIMER
TIMES: .BLKW 1000. ;BUFFER AREA TO SAVE SERVO EXECUTION TIME
.ENDC
.IFNZ FRCDAT
FDATA: .BLKW 2000. ;FORCE SENSING SYSTEM DATA BUFFER AREA
.ENDC
GIAREA: .BLKW 4. ;INTEGER STORAGE FOR GATHER
;GAREA: .BLKW 2*500. ;FP STORAGE FOR GATHER
GAREA: 0 ; changed to 0 by MSM
ARMEND: